| rviz中的标记（markers）和交互标记（interactive markers） | 您所在的位置：网站首页 › 可以自己做标记的地图插件 › rviz中的标记（markers）和交互标记（interactive markers） |
rviz中的标记（markers）和交互标记（interactive markers）
|
通过发布合适的话题可以在rviz的视图中显示合适的话题。可以在example_rviz_marker中查看例子example_rviz_marker.cpp。 #include #include // need this for publishing markers #include //data type used for markers #include #include #include //自定义的消息类型 using namespace std; //set these two values by service callback, make available to "main" double g_z_height = 0.0; bool g_trigger = true; //a service to prompt a new display computation. // E.g., to construct a plane at height z=1.0, trigger with: // rosservice call rviz_marker_svc 1.0 bool displaySvcCB(example_rviz_marker::SimpleFloatSrvMsgRequest& request, example_rviz_marker::SimpleFloatSrvMsgResponse& response) { g_z_height = request.request_float32; ROS_INFO("example_rviz_marker: received request for height %f", g_z_height); g_trigger = true; // inform "main" a new computation is desired response.resp=true; return true; } void init_marker_vals(visualization_msgs::Marker &marker) { marker.header.frame_id = "/world"; // reference frame for marker coords marker.header.stamp = ros::Time(); marker.ns = "my_namespace"; marker.id = 0; // use SPHERE if you only want a single marker // use SPHERE_LIST for a group of markers marker.type = visualization_msgs::Marker::SPHERE_LIST; //SPHERE; marker.action = visualization_msgs::Marker::ADD; // if just using a single marker, specify the coordinates here, like this: //marker.pose.position.x = 0.4; //marker.pose.position.y = -0.4; //marker.pose.position.z = 0; //ROS_INFO("x,y,z = %f %f, %f",marker.pose.position.x,marker.pose.position.y, // marker.pose.position.z); // otherwise, for a list of markers, put their coordinates in the "points" array, as below //whether a single marker or list of markers, need to specify marker properties // these will all be the same for SPHERE_LIST marker.pose.orientation.x = 0.0; marker.pose.orientation.y = 0.0; marker.pose.orientation.z = 0.0; marker.pose.orientation.w = 1.0; marker.scale.x = 0.02; marker.scale.y = 0.02; marker.scale.z = 0.02; marker.color.a = 1.0; marker.color.r = 1.0; marker.color.g = 0.0; marker.color.b = 0.0; } int main(int argc, char **argv) { ros::init(argc, argv, "example_rviz_marker"); ros::NodeHandle nh; ros::Publisher vis_pub = nh.advertise("example_marker_topic", 0); visualization_msgs::Marker marker; // 实例化一个标记对象 geometry_msgs::Point point; // 点云用来表示标记点的位置 //set up a service to compute marker locations on request设置一个服务计算所请求的标记点的位置 ros::ServiceServer service = nh.advertiseService("rviz_marker_svc", displaySvcCB); init_marker_vals(marker);//调用预定义函数，初始化标记点 double z_des; // build a wall of markers; set range and resolution建立标记墙，设置范围和分辨率 double x_min = -1.0; double x_max = 1.0; double y_min = -1.0; double y_max = 1.0; double dx_des = 0.1; double dy_des = 0.1; while (ros::ok()) { if (g_trigger) { // did service get request for a new computation? g_trigger = false; //reset the trigger from service z_des = g_z_height; //use z-value from service callback ROS_INFO("constructing plane of markers at height %f",z_des); marker.header.stamp = ros::Time(); marker.points.clear(); // clear out this vector for (double x_des = x_min; x_des < x_max; x_des += dx_des) { for (double y_des = y_min; y_des < y_max; y_des += dy_des) { point.x = x_des; point.y = y_des; point.z = z_des; marker.points.push_back(point); } } } ros::Duration(0.1).sleep(); //ROS_INFO("publishing..."); vis_pub.publish(marker); ros::spinOnce(); } return 0; }节点的源代码定义了一个服务rviz_marker_svc。服务使用自定义的消息SimpleFloatSrvMsg，期望客户端提供一个单浮点数。服务可以通过以下命令手动调用： rosservice call rviz_marker_svc 1.0当服务被调用的时候，example_rviz_maker节点将在高度为1.0的位置计算一系列的平面点。 运行这个例子，首先启动roscore，然后输入 rosrun example_rviz_marker example_rv_marker启动rviz rosrun rviz rviz选择world为固定坐标系，添加Marker组件，选择Marker Topic为/example_marker_topic，可以观察到平面标记点信息 标记点向上平移了一个单位 先上triad_display.cpp代码 // triad_display.cpp // Wyatt Newman, 8/16 // node to assist display of triads (axes) in rviz // this node subscribes to topic "triad_display_pose", from which it receives geometry_msgs/PoseStamped poses // it uses this info to populate and publish axes, using whatever frame_id is in the pose header // To see the result, add a "Marker" display in rviz and subscribe to the marker topic "/triad_display" // Can test this display node with the test node: "triad_display_test_node", which generates moving poses // corresponding to a marker origin spiraling up in z #include #include //#include #include #include #include #include #include #include #include #include #include //some globals... geometry_msgs::Point vertex1; geometry_msgs::PoseStamped g_stamped_pose; Eigen::Affine3d g_affine_marker_pose; // create arrow markers; do this 3 times to create a triad (frame) visualization_msgs::Marker arrow_marker_x; //this one for the x axis visualization_msgs::Marker arrow_marker_y; //this one for the y axis visualization_msgs::Marker arrow_marker_z; //this one for the y axis //udpdate_arrows() set the frame and void update_arrows() { geometry_msgs::Point origin, arrow_x_tip, arrow_y_tip, arrow_z_tip; Eigen::Matrix3d R; Eigen::Quaterniond quat; quat.x() = g_stamped_pose.pose.orientation.x; quat.y() = g_stamped_pose.pose.orientation.y; quat.z() = g_stamped_pose.pose.orientation.z; quat.w() = g_stamped_pose.pose.orientation.w; R = quat.toRotationMatrix(); Eigen::Vector3d x_vec, y_vec, z_vec; double veclen = 0.2; //make the arrows this long x_vec = R.col(0) * veclen; y_vec = R.col(1) * veclen; z_vec = R.col(2) * veclen; //update the arrow markers w/ new pose: origin = g_stamped_pose.pose.position; arrow_x_tip = origin; arrow_x_tip.x += x_vec(0); arrow_x_tip.y += x_vec(1); arrow_x_tip.z += x_vec(2); arrow_marker_x.points.clear(); arrow_marker_x.points.push_back(origin); arrow_marker_x.points.push_back(arrow_x_tip); arrow_marker_x.header = g_stamped_pose.header; arrow_y_tip = origin; arrow_y_tip.x += y_vec(0); arrow_y_tip.y += y_vec(1); arrow_y_tip.z += y_vec(2); arrow_marker_y.points.clear(); arrow_marker_y.points.push_back(origin); arrow_marker_y.points.push_back(arrow_y_tip); arrow_marker_y.header = g_stamped_pose.header; arrow_z_tip = origin; arrow_z_tip.x += z_vec(0); arrow_z_tip.y += z_vec(1); arrow_z_tip.z += z_vec(2); arrow_marker_z.points.clear(); arrow_marker_z.points.push_back(origin); arrow_marker_z.points.push_back(arrow_z_tip); arrow_marker_z.header = g_stamped_pose.header; } //init persistent params of markers, then variable coords void init_markers() { //initialize stamped pose for at a legal (if boring) pose g_stamped_pose.header.stamp = ros::Time::now(); g_stamped_pose.header.frame_id = "world"; g_stamped_pose.pose.position.x = 0; g_stamped_pose.pose.position.y = 0; g_stamped_pose.pose.position.z = 0; g_stamped_pose.pose.orientation.x = 0; g_stamped_pose.pose.orientation.y = 0; g_stamped_pose.pose.orientation.z = 0; g_stamped_pose.pose.orientation.w = 1; //the following parameters only need to get set once arrow_marker_x.type = visualization_msgs::Marker::ARROW; arrow_marker_x.action = visualization_msgs::Marker::ADD; //create or modify marker arrow_marker_x.ns = "triad_namespace"; arrow_marker_x.lifetime = ros::Duration(); //never delete // make the arrow thin arrow_marker_x.scale.x = 0.01; arrow_marker_x.scale.y = 0.01; arrow_marker_x.scale.z = 0.01; arrow_marker_x.color.r = 1.0; // red, for the x axis arrow_marker_x.color.g = 0.0; arrow_marker_x.color.b = 0.0; arrow_marker_x.color.a = 1.0; arrow_marker_x.id = 0; arrow_marker_x.header = g_stamped_pose.header; //y and z arrow params are the same, except for colors arrow_marker_y = arrow_marker_x; arrow_marker_y.color.r = 0.0; arrow_marker_y.color.g = 1.0; //green for y axis arrow_marker_y.color.b = 0.0; arrow_marker_y.color.a = 1.0; arrow_marker_y.id = 1; arrow_marker_z = arrow_marker_x; arrow_marker_z.id = 2; arrow_marker_z.color.r = 0.0; arrow_marker_z.color.g = 0.0; arrow_marker_z.color.b = 1.0; //blue for z axis arrow_marker_z.color.a = 1.0; //set the poses of the arrows based on g_stamped_pose update_arrows(); } void poseCB(const geometry_msgs::PoseStamped &pose_msg) { ROS_DEBUG("got pose message"); //ROS_INFO("got pose message"); g_stamped_pose.header = pose_msg.header; g_stamped_pose.pose = pose_msg.pose; } int main(int argc, char** argv) { ros::init(argc, argv, "triad_display"); // this will be the node name; ros::NodeHandle nh; // subscribe to stamped-pose publications ros::Subscriber pose_sub = nh.subscribe("triad_display_pose", 1, poseCB); ros::Publisher vis_pub = nh.advertise("triad_display", 1); init_markers(); ros::Rate timer(20); //timer to run at 20 Hz while (ros::ok()) { update_arrows(); vis_pub.publish(arrow_marker_x); //publish the marker ros::Duration(0.01).sleep(); vis_pub.publish(arrow_marker_y); //publish the marker ros::Duration(0.01).sleep(); vis_pub.publish(arrow_marker_z); //publish the marker ros::spinOnce(); //let callbacks perform an update timer.sleep(); } }依次运行以下命令： roscore rosrun example_rviz_marker triad _display rosrun rviz rviz rosrun example_rviz_marker triad_display_test_node可以看到一个直角坐标标记螺旋上升。 rviz中的交互式标记（interactive marker）交互式标记的例子可以在example_interactive_marker包中找到。以下是示例代码 // IM_6DOF_example.cpp // Wyatt Newman, based on ROS tutorial 4.2 on Interactive Markers #include #include #include #include #include const int IM_GET_CURRENT_MARKER_POSE = 0; const int IM_SET_NEW_MARKER_POSE = 1; geometry_msgs::Point g_current_point; geometry_msgs::Quaternion g_current_quaternion; ros::Time g_marker_time; interactive_markers::InteractiveMarkerServer *g_IM_server; //("rt_hand_marker"); visualization_msgs::InteractiveMarkerFeedback *g_IM_feedback; //service: return pose of marker from above globals; // depending on mode, move IM programmatically, bool IM6DofSvcCB(example_interactive_marker::ImNodeSvcMsgRequest& request, example_interactive_marker::ImNodeSvcMsgResponse& response) { //if busy, refuse new requests; // for a simple status query, handle it now; if (request.cmd_mode == IM_GET_CURRENT_MARKER_POSE) { ROS_INFO("IM6DofSvcCB: rcvd request for query--GET_CURRENT_MARKER_POSE"); response.poseStamped_IM_current.header.stamp = g_marker_time; response.poseStamped_IM_current.header.frame_id = "world"; response.poseStamped_IM_current.pose.position = g_current_point;//返回位置 response.poseStamped_IM_current.pose.orientation = g_current_quaternion;//返回方向 return true; } //command to move the marker to specified pose: if (request.cmd_mode == IM_SET_NEW_MARKER_POSE) { geometry_msgs::PoseStamped poseStamped_IM_desired; ROS_INFO("IM6DofSvcCB: rcvd request for action--SET_NEW_MARKER_POSE"); g_current_point = request.poseStamped_IM_desired.pose.position;//设置位置 g_current_quaternion = request.poseStamped_IM_desired.pose.orientation;//设置当前方向 g_marker_time = ros::Time::now(); poseStamped_IM_desired = request.poseStamped_IM_desired; poseStamped_IM_desired.header.stamp = g_marker_time; response.poseStamped_IM_current = poseStamped_IM_desired; //g_IM_feedback->pose = poseStamped_IM_desired.pose; response.poseStamped_IM_current.header.stamp = g_marker_time; response.poseStamped_IM_current.header.frame_id = "torso"; response.poseStamped_IM_current.pose.position = g_current_point; response.poseStamped_IM_current.pose.orientation = g_current_quaternion; g_IM_server->setPose("des_hand_pose", poseStamped_IM_desired.pose); //g_IM_feedback->marker_name,poseStamped_IM_desired.pose); g_IM_server->applyChanges(); return true; } ROS_WARN("IM6DofSvcCB: case not recognized"); return false; } void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) { ROS_INFO_STREAM(feedback->marker_name |
调用服务
【本文地址】
公司简介
联系我们