实现在rviz中画出圆形轨迹

最新推荐文章于 2024-04-10 19:50:45 发布

转载最新推荐文章于 2024-04-10 19:50:45 发布 · 2k 阅读

Linux 专栏收录该内容

52 篇文章

订阅专栏

本文介绍了一个使用ROS（Robot Operating System）进行路径数据发布和订阅的详细示例。通过创建一个名为showpath的包，该示例展示了如何利用nav_msgs/Path消息类型在trajectory主题上发布机器人路径数据。同时，文章提供了showpath.cpp源代码的编辑指导，以及CMakeLists.txt的配置说明，以便于完成项目的构建。

参考：https://blog.youkuaiyun.com/ktigerhero3/article/details/70256437

cd ./catkin_ws/src
catkin_create_pkg showpath roscpp rospy sensor_msgs std_msgs nav_msgs tf
cd ./showpath/src
tounch showpath.cpp

编辑主函数showpath.cpp

#include <ros/ros.h> 
#include <ros/console.h> 
#include <nav_msgs/Path.h> 
#include <std_msgs/String.h> 
#include <geometry_msgs/Quaternion.h> 
#include <geometry_msgs/PoseStamped.h> 
#include <tf/transform_broadcaster.h> 
#include <tf/tf.h>
main (int argc, char **argv)
{ 
  ros::init (argc, argv, "showpath"); 

  ros::NodeHandle ph; 
  ros::Publisher path_pub = ph.advertise<nav_msgs::Path>("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 = (current_time - last_time).toSec(); 
   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; 
}

编辑CMakeLists.txt
在最后加入

add_executable(showpath src/showpath.cpp)
target_link_libraries(showpath ${catkin_LIBRARIES})

编译catkin_make --pkg showpath