ROS RViz基本学习笔记（一）
目标：利用ROS RViz写出一个可以实现可视化的视觉自定位3D展示
环境： ubuntu16.04,ROS_kinetic
前期工作：配置好相关环境，可以跑通RViz/Tutorials
链接：http://wiki.ros.org/rviz/Tutorials/Markers%3A%20Basic%20Shapes
一 、初始化
catkin_create_pkg mapping roscpp visualization_msgs
cd src/mapping/src
gedit mapping.cpp
二、程序部分
#include
#include
#include
main(int argc, char** argv){
ros::init(argc , argv , "mapping");
ros::NodeHandle n;
ros::Rate per(10);
ros::Publisher pub = n.advertise("mapping_marker",1000);
while(ros::ok()){
visualization_msgs::Marker points,lines[11],circle;
visualization_msgs::Marker door[2];
points.header.frame_id = circle.header.frame_id = door[0].header.frame_id = door[1].header.frame_id = "/my_frame";
for(int lines_num = 0;lines_num < 11;lines_num++){
lines[lines_num].header.frame_id = "/my_frame";
}
points.header.stamp = circle.header.stamp = door[0].header.stamp = door[1].header.stamp = ros::Time::now();
for(int lines_num = 0;lines_num < 11;lines_num++){
lines[lines_num].header.stamp = ros::Time::now();
}
points.ns = circle.ns = door[0].ns = door[1].ns = "mapping";
for(int lines_num = 0;lines_num < 11;lines_num++){
lines[lines_num].ns = "mapping";
}
points.action = circle.action = door[0].action = door[1].action = visualization_msgs::Marker::ADD;
for(int lines_num = 0;lines_num < 11;lines_num++){
lines[lines_num].action = visualization_msgs::Marker::ADD;
}
points.id = 0;
circle.id = 1;
door[0].id = 2;
door[1].id = 3;
for(int lines_num = 0;lines_num < 11;lines_num++){
lines[lines_num].id = 4+lines_num;
}
points.type = visualization_msgs::Marker::POINTS;
for(int lines_num = 0;lines_num < 11;lines_num++){
lines[lines_num].type = visualization_msgs::Marker::LINE_STRIP;
}
circle.type = visualization_msgs::Marker::LINE_LIST;
door[0].type = visualization_msgs::Marker::CYLINDER;
door[1].type = visualization_msgs::Marker::CYLINDER;
/////////////////////////////////////////init of points and lines
points.scale.x = 0.2;
points.scale.y = 0.2;
points.color.g = 1.0;
points.color.a = 1.0f;
for(int lines_num = 0;lines_num < 11;lines_num++){
lines[lines_num].scale.x = 0.1;
lines[lines_num].scale.y = 0.1;
lines[lines_num].color.r = 1.0;
lines[lines_num].color.g = 1.0;
lines[lines_num].color.b = 1.0;
lines[lines_num].color.a = 1.0;
}
circle.scale.x = 0.1;
circle.scale.y = 0.1;
circle.color.b = 1.0;
circle.color.a = 1.0;
geometry_msgs::Point p1,p2,p3,p4,p5,p6,p7,p8,p9,p10,p11,p12,p13,p14;
p1.x = 4.5;p1.y = 6;p1.z = 0;
p2.x = 4.5;p2.y = -6;p2.z = 0;
p3.x = -4.5;p3.y = -6;p3.z = 0;
p4.x = -4.5;p4.y = 6;p4.z = 0;
p5.x = -3; p5.y = 6;p5.z = 0;
p6.x = 3;p6.y = 6;p6.z = 0;
p7.x = 3;p7.y = 5;p7.z = 0;
p8.x = -3;p8.y = 5;p8.z = 0;
p9.x = 4.5;p9.y = 0;p9.z = 0;
p10.x = 3;p10.y = -6;p10.z = 0;
p11.x = -3;p11.y = -6;p11.z = 0;
p12.x = -3;p12.y = -5;p12.z = 0;
p13.x = 3;p13.y = -5; p13.z = 0;
p14.x = -4.5; p14.y =0;p14.z =0;
points.points.push_back(p1);
points.points.push_back(p2);
points.points.push_back(p3);
points.points.push_back(p4);
points.points.push_back(p5);
points.points.push_back(p6);
points.points.push_back(p7);
points.points.push_back(p8);
points.points.push_back(p9);
points.points.push_back(p10);
points.points.push_back(p11);
points.points.push_back(p12);
points.points.push_back(p13);
points.points.push_back(p14);
lines[0].points.push_back(p1);
lines[0].points.push_back(p2);
lines[1].points.push_back(p2);
lines[1].points.push_back(p3);
lines[2].points.push_back(p3);
lines[2].points.push_back(p4);
lines[3].points.push_back(p4);
lines[3].points.push_back(p1);
lines[4].points.push_back(p9);
lines[4].points.push_back(p14);
lines[5].points.push_back(p5);
lines[5].points.push_back(p8);
lines[6].points.push_back(p8);
lines[6].points.push_back(p7);
lines[7].points.push_back(p7);
lines[7].points.push_back(p6);
lines[8].points.push_back(p10);
lines[8].points.push_back(p13);
lines[9].points.push_back(p13);
lines[9].points.push_back(p12);
lines[10].points.push_back(p12);
lines[10].points.push_back(p11);
for(int i=0; i<360; i++){
geometry_msgs::Point p;
p.x = sin(i*2*M_PI/360);
p.y = cos(i*2*M_PI/360);
p.z = 0;
circle.points.push_back(p);
}
////////////////////////////////////////////init of door:
door[0].pose.position.x = 2;
door[0].pose.position.y = 6;
door[0].pose.position.z = -0.5;
door[1].pose.position.x = -2;
door[1].pose.position.y = 6;
door[1].pose.position.z = -0.5;
door[0].scale.x = 0.25;
door[0].scale.y = 0.25;
door[0].scale.z = 1.0;
door[1].scale.x = 0.25;
door[1].scale.y = 0.25;
door[1].scale.z = 1.0;
door[0].color.r = 0.0f;
door[0].color.g = 1.0f;
door[0].color.b = 0.0f;
door[0].color.a = 1.0;
door[1].color.r = 0.0f;
door[1].color.g = 1.0f;
door[1].color.b = 0.0f;
door[1].color.a = 1.0;
door[0].pose.orientation.x = 90;
door[0].pose.orientation.y = 0.0;
door[0].pose.orientation.z = 0.0;
door[1].pose.orientation.x = 90;
door[1].pose.orientation.y = 0.0;
door[1].pose.orientation.z = 0.0;
door[0].lifetime = ros::Duration();
door[1].lifetime = ros::Duration();
for(int lines_num = 0;lines_num < 11;lines_num++){
pub.publish(lines[lines_num]);
}
pub.publish(points);
pub.publish(circle);
pub.publish(door[0]);
pub.publish(door[1]);
per.sleep();
}
} 三、调试问题：
1/ 当直接运行rviz后，会出现global statu中报错：
“No tf data,Actual error,Fixed Frame [map] does not exit"
此时需要另外开终端输入
rosrun tf static_transform_publisher 0 0 0 0 0 1 map my_frame 10
重新启动rviz后问题得到解决。
效果图：
四、进阶版：
为了使能够程序实现从别的包接收消息并画出来，我们还需要加入一个subscriber来监听某一话题的消息，这里我们为了方便起见选择接收turtle1/pose的位置信息，并将其在图上画出：
代码：
#include
#include
#include
#include
#include
#include
using namespace std;
double recdata[3];
void recfunc(const turtlesim::Pose&msg){
//ROS_INFO_STREAM(std::setprecision(2)<("mapping_marker",1000);
ros::Subscriber sub = n.subscribe("turtle1/pose",1000,&recfunc);
while(ros::ok()){
visualization_msgs::Marker points,lines[11],circle;
visualization_msgs::Marker door[2];
visualization_msgs::Marker now_point;
points.header.frame_id = circle.header.frame_id = door[0].header.frame_id = door[1].header.frame_id = "/my_frame";
for(int lines_num = 0;lines_num < 11;lines_num++){
lines[lines_num].header.frame_id = "/my_frame";
}
now_point.header.frame_id = "/my_frame";
points.header.stamp = circle.header.stamp = door[0].header.stamp = door[1].header.stamp = ros::Time::now();
for(int lines_num = 0;lines_num < 11;lines_num++){
lines[lines_num].header.stamp = ros::Time::now();
}
now_point.header.stamp = ros::Time::now();
points.ns = circle.ns = door[0].ns = door[1].ns = "mapping";
for(int lines_num = 0;lines_num < 11;lines_num++){
lines[lines_num].ns = "mapping";
}
now_point.ns = "mapping" ;
points.action = circle.action = door[0].action = door[1].action = visualization_msgs::Marker::ADD;
for(int lines_num = 0;lines_num < 11;lines_num++){
lines[lines_num].action = visualization_msgs::Marker::ADD;
}
now_point.action = visualization_msgs::Marker::ADD;
points.id = 0;
circle.id = 1;
door[0].id = 2;
door[1].id = 3;
for(int lines_num = 0;lines_num < 11;lines_num++){
lines[lines_num].id = 4+lines_num;
}
now_point.id = 15;
points.type = visualization_msgs::Marker::POINTS;
for(int lines_num = 0;lines_num < 11;lines_num++){
lines[lines_num].type = visualization_msgs::Marker::LINE_STRIP;
}
circle.type = visualization_msgs::Marker::LINE_LIST;
door[0].type = visualization_msgs::Marker::CYLINDER;
door[1].type = visualization_msgs::Marker::CYLINDER;
now_point.type = visualization_msgs::Marker::POINTS;
/////////////////////////////////////////init of points and lines
points.scale.x = 0.2;
points.scale.y = 0.2;
points.color.g = 1.0;
points.color.a = 1.0f;
for(int lines_num = 0;lines_num < 11;lines_num++){
lines[lines_num].scale.x = 0.1;
lines[lines_num].scale.y = 0.1;
lines[lines_num].color.r = 1.0;
lines[lines_num].color.g = 1.0;
lines[lines_num].color.b = 1.0;
lines[lines_num].color.a = 1.0;
}
circle.scale.x = 0.1;
circle.scale.y = 0.1;
circle.color.b = 1.0;
circle.color.a = 1.0;
now_point.scale.x = 0.5;
now_point.scale.y = 0.5;
now_point.color.g = 1.0;
now_point.color.a = 1.0f;
geometry_msgs::Point p1,p2,p3,p4,p5,p6,p7,p8,p9,p10,p11,p12,p13,p14;
p1.x = 4.5;p1.y = 6;p1.z = 0;
p2.x = 4.5;p2.y = -6;p2.z = 0;
p3.x = -4.5;p3.y = -6;p3.z = 0;
p4.x = -4.5;p4.y = 6;p4.z = 0;
p5.x = -3; p5.y = 6;p5.z = 0;
p6.x = 3;p6.y = 6;p6.z = 0;
p7.x = 3;p7.y = 5;p7.z = 0;
p8.x = -3;p8.y = 5;p8.z = 0;
p9.x = 4.5;p9.y = 0;p9.z = 0;
p10.x = 3;p10.y = -6;p10.z = 0;
p11.x = -3;p11.y = -6;p11.z = 0;
p12.x = -3;p12.y = -5;p12.z = 0;
p13.x = 3;p13.y = -5; p13.z = 0;
p14.x = -4.5; p14.y =0;p14.z =0;
geometry_msgs::Point thispoint;
//将两坐标进行全映射
thispoint.x = -(0.818*recdata[0]-4.5);
thispoint.y = -(-1.091*recdata[1]+6);
thispoint.z = 0;
now_point.points.push_back(thispoint);
points.points.push_back(p1);
points.points.push_back(p2);
points.points.push_back(p3);
points.points.push_back(p4);
points.points.push_back(p5);
points.points.push_back(p6);
points.points.push_back(p7);
points.points.push_back(p8);
points.points.push_back(p9);
points.points.push_back(p10);
points.points.push_back(p11);
points.points.push_back(p12);
points.points.push_back(p13);
points.points.push_back(p14);
lines[0].points.push_back(p1);
lines[0].points.push_back(p2);
lines[1].points.push_back(p2);
lines[1].points.push_back(p3);
lines[2].points.push_back(p3);
lines[2].points.push_back(p4);
lines[3].points.push_back(p4);
lines[3].points.push_back(p1);
lines[4].points.push_back(p9);
lines[4].points.push_back(p14);
lines[5].points.push_back(p5);
lines[5].points.push_back(p8);
lines[6].points.push_back(p8);
lines[6].points.push_back(p7);
lines[7].points.push_back(p7);
lines[7].points.push_back(p6);
lines[8].points.push_back(p10);
lines[8].points.push_back(p13);
lines[9].points.push_back(p13);
lines[9].points.push_back(p12);
lines[10].points.push_back(p12);
lines[10].points.push_back(p11);
for(int i=0; i<360; i++){
geometry_msgs::Point p;
p.x = sin(i*2*M_PI/360);
p.y = cos(i*2*M_PI/360);
p.z = 0;
circle.points.push_back(p);
}
////////////////////////////////////////////init of door:
door[0].pose.position.x = 2;
door[0].pose.position.y = 6;
door[0].pose.position.z = -0.5;
door[1].pose.position.x = -2;
door[1].pose.position.y = 6;
door[1].pose.position.z = -0.5;
door[0].scale.x = 0.25;
door[0].scale.y = 0.25;
door[0].scale.z = 1.0;
door[1].scale.x = 0.25;
door[1].scale.y = 0.25;
door[1].scale.z = 1.0;
door[0].color.r = 0.0f;
door[0].color.g = 1.0f;
door[0].color.b = 0.0f;
door[0].color.a = 1.0;
door[1].color.r = 0.0f;
door[1].color.g = 1.0f;
door[1].color.b = 0.0f;
door[1].color.a = 1.0;
door[0].pose.orientation.x = 90;
door[0].pose.orientation.y = 0.0;
door[0].pose.orientation.z = 0.0;
door[1].pose.orientation.x = 90;
door[1].pose.orientation.y = 0.0;
door[1].pose.orientation.z = 0.0;
door[0].lifetime = ros::Duration();
door[1].lifetime = ros::Duration();
for(int lines_num = 0;lines_num < 11;lines_num++){
pub.publish(lines[lines_num]);
}
pub.publish(points);
pub.publish(circle);
pub.publish(door[0]);
pub.publish(door[1]);
pub.publish(now_point);
//cout<<"receive data = "<< recdata << endl;
ros::spinOnce();
per.sleep();
}
} 效果图：
如图可以实现将左侧遥控小乌龟的行走实时映射至我们构建的地图上。至此基本完成了预设目标。