通过发布合适的话题可以在rviz的视图中显示合适的话题。可以在example_rviz_marker中查看例子example_rviz_marker.cpp。
#include <ros/ros.h>
#include <visualization_msgs/Marker.h> // need this for publishing markers
#include <geometry_msgs/Point.h> //data type used for markers
#include <string.h>
#include <stdio.h>
#include <example_rviz_marker/SimpleFloatSrvMsg.h> //自定义的消息类型
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<visualization_msgs::Marker>("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,可以观察到平面标记点信息
调用服务
rosservice call rviz_marker_svc 1.0
标记点向上平移了一个单位
三维显示示例
先上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 <ros/ros.h>
#include <visualization_msgs/Marker.h>
//#include <visualization_msgs/InteractiveMarkerFeedback.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <math.h>
#include <Eigen/Eigen>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <tf_conversions/tf_eigen.h>
//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<visualization_msgs::Marker>("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 <ros/ros.h>
#include <iostream>
#include <interactive_markers/interactive_marker_server.h>
#include <geometry_msgs/Point.h>
#include <example_interactive_marker/ImNodeSvcMsg.h>
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 << " is now at "
<< feedback->pose.position.x << ", " << feedback->pose.position.y
<< ", " << feedback->pose.position.z);
g_current_quaternion = feedback->pose.orientation;
g_current_point = feedback->pose.position;
g_marker_time = ros::Time::now();
}
void init_arrow_marker_x(visualization_msgs::Marker &arrow_marker_x) {
geometry_msgs::Point temp_point;
arrow_marker_x.type = visualization_msgs::Marker::ARROW; //ROS example was a CUBE; changed to ARROW
// specify/push-in the origin point for the arrow
temp_point.x = temp_point.y = temp_point.z = 0;
arrow_marker_x.points.push_back(temp_point);
// Specify and push in the end point for the arrow
temp_point = g_current_point;
temp_point.x = 0.2; // arrow is this long in x direction
temp_point.y = 0.0;
temp_point.z = 0.0;
arrow_marker_x.points.push_back(temp_point);
// make the arrow very 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;
}
void init_arrow_marker_y(visualization_msgs::Marker &arrow_marker_y) {
geometry_msgs::Point temp_point;
arrow_marker_y.type = visualization_msgs::Marker::ARROW;
// Push in the origin point for the arrow
temp_point.x = temp_point.y = temp_point.z = 0;
arrow_marker_y.points.push_back(temp_point);
// Push in the end point for the arrow
temp_point.x = 0.0;
temp_point.y = 0.2; // points in the y direction
temp_point.z = 0.0;
arrow_marker_y.points.push_back(temp_point);
arrow_marker_y.scale.x = 0.01;
arrow_marker_y.scale.y = 0.01;
arrow_marker_y.scale.z = 0.01;
arrow_marker_y.color.r = 0.0;
arrow_marker_y.color.g = 1.0; // color it green, for y axis
arrow_marker_y.color.b = 0.0;
arrow_marker_y.color.a = 1.0;
}
void init_arrow_marker_z(visualization_msgs::Marker &arrow_marker_z) {
geometry_msgs::Point temp_point;
arrow_marker_z.type = visualization_msgs::Marker::ARROW; //CUBE;
// Push in the origin point for the arrow
temp_point.x = temp_point.y = temp_point.z = 0;
arrow_marker_z.points.push_back(temp_point);
// Push in the end point for the arrow
temp_point.x = 0.0;
temp_point.y = 0.0;
temp_point.z = 0.2;
arrow_marker_z.points.push_back(temp_point);
arrow_marker_z.scale.x = 0.01;
arrow_marker_z.scale.y = 0.01;
arrow_marker_z.scale.z = 0.01;
arrow_marker_z.color.r = 0.0;
arrow_marker_z.color.g = 0.0;
arrow_marker_z.color.b = 1.0;
arrow_marker_z.color.a = 1.0;
}
void init_translate_control_x(visualization_msgs::InteractiveMarkerControl &translate_control_x) {
translate_control_x.name = "move_x";
translate_control_x.interaction_mode =
visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
}
void init_translate_control_y(visualization_msgs::InteractiveMarkerControl &translate_control_y) {
translate_control_y.name = "move_y";
translate_control_y.interaction_mode =
visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
translate_control_y.orientation.x = 0; //point this in the y direction
translate_control_y.orientation.y = 0;
translate_control_y.orientation.z = 1;
translate_control_y.orientation.w = 1;
}
void init_translate_control_z(visualization_msgs::InteractiveMarkerControl &translate_control_z) {
translate_control_z.name = "move_z";
translate_control_z.interaction_mode =
visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
translate_control_z.orientation.x = 0; //point this in the y direction
translate_control_z.orientation.y = 1;
translate_control_z.orientation.z = 0;
translate_control_z.orientation.w = 1;
}
void init_rotx_control(visualization_msgs::InteractiveMarkerControl &rotx_control) {
rotx_control.always_visible = true;
rotx_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
rotx_control.orientation.x = 1;
rotx_control.orientation.y = 0;
rotx_control.orientation.z = 0;
rotx_control.orientation.w = 1;
rotx_control.name = "rot_x";
}
void init_roty_control(visualization_msgs::InteractiveMarkerControl &roty_control) {
roty_control.always_visible = true;
roty_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
roty_control.orientation.x = 0;
roty_control.orientation.y = 0;
roty_control.orientation.z = 1;
roty_control.orientation.w = 1;
roty_control.name = "rot_y";
}
void init_rotz_control(visualization_msgs::InteractiveMarkerControl &rotz_control) {
rotz_control.always_visible = true;
rotz_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
rotz_control.orientation.x = 0;
rotz_control.orientation.y = 1;
rotz_control.orientation.z = 0;
rotz_control.orientation.w = 1;
rotz_control.name = "rot_z";
}
void init_IM_control(visualization_msgs::InteractiveMarkerControl &IM_control,
visualization_msgs::Marker &arrow_marker_x,
visualization_msgs::Marker &arrow_marker_y, visualization_msgs::Marker &arrow_marker_z) {
init_arrow_marker_x(arrow_marker_x); //set up arrow params for x
init_arrow_marker_y(arrow_marker_y); //set up arrow params for y
init_arrow_marker_z(arrow_marker_z); //set up arrow params for z
IM_control.always_visible = true;
IM_control.markers.push_back(arrow_marker_x);
IM_control.markers.push_back(arrow_marker_y);
IM_control.markers.push_back(arrow_marker_z);
}
void init_int_marker(visualization_msgs::InteractiveMarker &int_marker) {
int_marker.header.frame_id = "world"; //base_link"; ///world"; // the reference frame for pose coordinates
int_marker.name = "des_hand_pose"; //name the marker
int_marker.description = "Interactive Marker";
/** Scale Down: this makes all of the arrows/disks for the user controls smaller than the default size */
int_marker.scale = 0.2;
/** specify/push-in the origin for this marker */
//let's pre-position the marker, else it will show up at the frame origin by default
int_marker.pose.position.x = g_current_point.x;
int_marker.pose.position.y = g_current_point.y;
int_marker.pose.position.z = g_current_point.z;
}
int main(int argc, char** argv) {
ros::init(argc, argv, "simple_marker"); // this will be the node name;
ros::NodeHandle nh; //standard ros node handle
// create an interactive marker server on the topic namespace simple_marker
interactive_markers::InteractiveMarkerServer server("rt_hand_marker");
g_IM_server = &server;
ros::ServiceServer IM_6dof_interface_service = nh.advertiseService("IM6DofSvc", &IM6DofSvcCB);
// look for resulting pose messages on the topic: /rt_hand_marker/feedback,
// which publishes a message of type visualization_msgs/InteractiveMarkerFeedback, which
// includes a full "pose" of the marker.
// Coordinates of the pose are with respect to the named frame
g_current_point.x = 0.5; //init these global values
g_current_point.y = -0.5; //will be used in subsequent init fncs
g_current_point.z = 0.2;
// create an interactive marker for our server
visualization_msgs::InteractiveMarker int_marker;
init_int_marker(int_marker);
// arrow markers; 3 to create a triad (frame)
visualization_msgs::Marker arrow_marker_x, arrow_marker_y, arrow_marker_z;
// create a control that contains the markers
visualization_msgs::InteractiveMarkerControl IM_control;
//initialize values for this control
init_IM_control(IM_control, arrow_marker_x, arrow_marker_y, arrow_marker_z);
// add the control to the interactive marker
int_marker.controls.push_back(IM_control);
// create a control that will move the marker
// this control does not contain any markers,
// which will cause RViz to insert two arrows
visualization_msgs::InteractiveMarkerControl translate_control_x,
translate_control_y, translate_control_z;
init_translate_control_x(translate_control_x);
init_translate_control_y(translate_control_y);
init_translate_control_z(translate_control_z);
// add x,y,and z-rotation controls
visualization_msgs::InteractiveMarkerControl rotx_control, roty_control,
rotz_control;
init_rotx_control(rotx_control);
init_roty_control(roty_control);
init_rotz_control(rotz_control);
// add the controls to the interactive marker
int_marker.controls.push_back(translate_control_x);
int_marker.controls.push_back(translate_control_y);
int_marker.controls.push_back(translate_control_z);
int_marker.controls.push_back(rotx_control);
int_marker.controls.push_back(rotz_control);
int_marker.controls.push_back(roty_control);
// add the interactive marker to our collection &
// tell the server to call processFeedback() when feedback arrives for it
//server.insert(int_marker, &processFeedback);
g_IM_server->insert(int_marker, &processFeedback);
// 'commit' changes and send to all clients
//server.applyChanges();
g_IM_server->applyChanges();
// start the ROS main loop
ROS_INFO("going into spin...");
ros::spin();
}
在前期的基础上运行
rosrun example_interactive_marker exale_interactive_marker_node
然后在rviz界面添加Interactive Marker组件,
可以看到
鼠标移动标记会显示出原点位置。
评论(0)
您还未登录,请登录后发表或查看评论