ROS在rviz中实时显示轨迹（nav |
您所在的位置：网站首页 › rviz设置小车大小 › ROS在rviz中实时显示轨迹（nav |
ROS在rviz中实时显示轨迹（nav
|
这么好的文章怎么能不转发…无改动~~ 参考（九）ROS在rviz中实时显示轨迹（nav_msgs/Path消息的使用） 如何在rviz中如何实时显示轨迹呢？ 本文分析nav_msgs/Path结构，实现在rviz中画出圆形轨迹。 1.使用参考代码画轨迹效果展示（hector_slam）google rviz中如何实时显示轨迹?会有以下重要资料。 参考资料 参考http://answers.ros.org/question/209224/show-robot-trajectory-in-rviz-real-time/ hector_slam中实现了画轨迹。 网址http://wiki.ros.org/hector_slam/Tutorials/MappingUsingLoggedData 那么先用bag数据来跑一跑。 步骤如下： （1）启动包 roslaunch hector_slam_launch tutorial.launch（2）回放bag rosbag play Team_Hector_MappingBox_RoboCup_2011_Rescue_Arena.bag --clock （3）效果
以上的包太复杂，本文实现最简单的画轨迹功能。 2.1分析nav_msgs/Path.msg结构nav_msgs/Path.msg结构 std_msgs/Header header geometry_msgs/PoseStamped[] poses将以上结构展开如下： std_msgs/Header结构 uint32 seq time stamp string frame_idgeometry_msgs/PoseStamped[]结构 std_msgs/Header header geometry_msgs/Pose posegeometry_msgs/Pose pose结构 # This represents an orientation in free space in quaternion form. float64 x float64 y float64 z float64 w 2.2实现画出圆形轨迹（1）新建工程 mkdir -p showpath/src cd src catkin_create_pkg showpath roscpp rospy sensor_msgs std_msgs nav_msgs tf cd .. catkin_make（2）编辑主函数showpath.cpp #include #include #include #include #include #include #include #include main (int argc, char **argv) { ros::init (argc, argv, "showpath"); ros::NodeHandle ph; ros::Publisher path_pub = ph.advertise("trajectory",1, true); ros::Time current_time, last_time; current_time = ros::Time::now(); last_time = ros::Time::now(); nav_msgs::Path path; //nav_msgs::Path path; path.header.stamp=current_time; path.header.frame_id="odom"; double x = 0.0; double y = 0.0; double th = 0.0; double vx = 0.1; double vy = -0.1; double vth = 0.1; ros::Rate loop_rate(1); while (ros::ok()) { current_time = ros::Time::now(); //compute odometry in a typical way given the velocities of the robot double dt = 0.1; double delta_x = (vx * cos(th) - vy * sin(th)) * dt; double delta_y = (vx * sin(th) + vy * cos(th)) * dt; double delta_th = vth * dt; x += delta_x; y += delta_y; th += delta_th; geometry_msgs::PoseStamped this_pose_stamped; this_pose_stamped.pose.position.x = x; this_pose_stamped.pose.position.y = y; geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(th); this_pose_stamped.pose.orientation.x = goal_quat.x; this_pose_stamped.pose.orientation.y = goal_quat.y; this_pose_stamped.pose.orientation.z = goal_quat.z; this_pose_stamped.pose.orientation.w = goal_quat.w; this_pose_stamped.header.stamp=current_time; this_pose_stamped.header.frame_id="odom"; path.poses.push_back(this_pose_stamped); path_pub.publish(path); ros::spinOnce(); // check for incoming messages last_time = current_time; loop_rate.sleep(); } return 0; }（3）编辑CMakeLists.txt 在最后加入 add_executable(showpath src/showpath.cpp) target_link_libraries(showpath ${catkin_LIBRARIES})（4）编译和运行 编译 cd /home/topeet/workspace/showpath catkin_make运行 source ./devel/setup.bash rosrun showpath showpath查看/trajectory 信息 rostopic echo /trajectory 3.rviz实验结果运行 rosrun rviz rviz在globel option的Fixed Fram输入odom 左边点击add 选中path 在path的topic选项中选 /trajectory |
（4）查看发送的topic
【本文地址】