devbox
weixin_40725706
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

运动控制入门到入土:pure persuit纯追踪算法gazebo仿真_ros插件实现纯跟踪算法

作者：weixin_40725706 | 2024-05-19 08:49:01

赞

踩

ros插件实现纯跟踪算法

本代码根据【从零开始自动驾驶】01 - 05_哔哩哔哩_bilibili 的逻辑和代码。写了一套C++代码

感谢B站up Imjusty的教学

cmdvel2gazebo.cpp

  1. #include "cmdvel2gazebo.h"
  2.  
  3. int main(int argc, char** argv){
  4.   ros::init(argc, argv, "cmdvel2gazebo");
  5.   cmdvel2gazebo cmdvel2gazebo;
  6.   cmdvel2gazebo.control();
  7.   return 0;
  8. }
  9.  
  10. void cmdvel2gazebo::Cmd_Vel_Callback(const geometry_msgs::Twist &twist){
  11.   velocity = twist.linear.x / 0.3;
  12.   // cout << "velocity:" << velocity <<endl;
  13.   angle = max(-maxsteer, min(maxsteer, twist.angular.z));
  14.   // cout << "angle:" << angle <<endl;
  15.   last_Time = ros::Time::now();
  16.   publish();
  17. }
  18.  
  19. void cmdvel2gazebo::control(){
  20.   ros::spin();
  21. }
  22.  
  23. void cmdvel2gazebo::publish(){
  24.   std_msgs::Float64 msg_vel_L, msg_vel_R, msg_ang_L, msg_ang_R;
  25.   delta_Time = (ros::Time::now()- last_Time).toSec();
  26.   msgs_too_old = delta_Time > timeout ? 1: 0;
  27.   if(msgs_too_old){
  28.     velocity = 0;
  29.     angle= 0;
  30.     msg_vel_L.data = velocity;
  31.     msg_vel_R.data = velocity;
  32.     msg_ang_L.data = angle;
  33.     msg_ang_R.data = angle;
  34.  
  35.     pub_rearL.publish(msg_vel_L);
  36.     pub_rearR.publish(msg_vel_R);
  37.     pub_steerL.publish(msg_ang_L);
  38.     pub_steerR.publish(msg_ang_R);
  39.   }
  40.  
  41.   // cout << "velocity1:" << velocity <<endl;
  42.   // cout << "angle1:" << angle <<endl;
  43.  
  44.   if(angle!= 0){
  45.     radius = wheelbase/fabs(tan(angle));
  46.     radius_rearL = radius- (copysign(1, angle)* (Tread/ 2.0));
  47.     radius_rearR = radius+ (copysign(1, angle)* (Tread/ 2.0));
  48.     radius_frontL = radius- (copysign(1, angle)* (Tread/ 2.0));
  49.     radius_frontR = radius+ (copysign(1, angle)* (Tread/ 2.0));
  50.  
  51.     velocity_rearL = velocity* radius_rearL/ radius;
  52.     velocity_rearR = velocity* radius_rearR/ radius;
  53.  
  54.     msg_vel_L.data = velocity_rearL;
  55.     msg_vel_R.data = velocity_rearR;
  56.  
  57.  
  58.     pub_rearL.publish(msg_vel_L);
  59.     pub_rearR.publish(msg_vel_R);
  60.  
  61.     angle_frontL = atan2(wheelbase, radius_frontL)* copysign(1, angle);
  62.     angle_frontR = atan2(wheelbase, radius_frontR)* copysign(1, angle);
  63.  
  64.     // cout << "angle_L:" << angle_frontL <<endl;
  65.     // cout << "angle_R:" << angle_frontR <<endl;
  66.  
  67.     msg_ang_L.data = angle_frontL;
  68.     msg_ang_R.data = angle_frontR;
  69.  
  70.     pub_steerL.publish(msg_ang_L);
  71.     pub_steerR.publish(msg_ang_R);
  72.   }
  73.   else{
  74.     msg_vel_L.data = velocity;
  75.     msg_vel_R.data = velocity;
  76.     msg_ang_L.data = angle;
  77.     msg_ang_R.data = angle;
  78.  
  79.     pub_rearL.publish(msg_vel_L);
  80.     pub_rearR.publish(msg_vel_R);
  81.     pub_steerL.publish(msg_ang_L);
  82.     pub_steerR.publish(msg_ang_R);
  83.   }
  84.  
  85. }
  86.  
  87. cmdvel2gazebo::cmdvel2gazebo()
  88. {
  89.   pub_steerL = n.advertise <std_msgs::Float64>("/smart/front_left_steering_position_controller/command", 10);
  90.   pub_steerR = n.advertise <std_msgs::Float64>("/smart/front_right_steering_position_controller/command", 10);
  91.   pub_rearL = n.advertise <std_msgs::Float64>("/smart/rear_left_velocity_controller/command",10);
  92.   pub_rearR = n.advertise <std_msgs::Float64>("/smart/rear_right_velocity_controller/command",10);
  93.   cmd_vel_sub = n.subscribe("/smart/cmd_vel", 100, &cmdvel2gazebo::Cmd_Vel_Callback, this);
  94.  
  95.  
  96.   last_Time= ros::Time::now();
  97.  
  98.  
  99.   rMax = wheelbase/ tan(maxsteerInside);
  100.   rIdeal = rMax+ (Tread/ 2.0);
  101.   maxsteer = atan2(wheelbase, rIdeal);
  102. }
  103.  
  104. cmdvel2gazebo::~cmdvel2gazebo()
  105. {
  106. }

transform_publisher.cpp

  1. #include "transform_publisher.h"
  2.  
  3. int main(int argc, char** argv){
  4.   ros::init(argc, argv, "transform_publisher");
  5.   transform_publisher transform_publisher;
  6.   transform_publisher.control();
  7. }
  8.  
  9. void transform_publisher::pose_callback(const geometry_msgs::PoseStamped &pose_msgs){
  10.   auto pose = pose_msgs.pose.position;
  11.   cout << pose << endl;
  12.   auto orientation = pose_msgs.pose.orientation;
  13.   static tf::TransformBroadcaster br;
  14.   tf::Transform transform;
  15.   transform.setOrigin(tf::Vector3(pose.x, pose.y, pose.z));
  16.   transform.setRotation(tf::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w));
  17.  
  18.   br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "base_link"));
  19. }
  20.  
  21. void transform_publisher::control(){
  22.   ros::spin();
  23. }
  24.  
  25. transform_publisher::transform_publisher(){
  26.   center_pose_sub = n.subscribe("/smart/center_pose", 10, &transform_publisher::pose_callback, this);
  27. }
  28.  
  29. transform_publisher::~transform_publisher(){
  30.  
  31. }

vehicle_pose_and_velocity_updater.cpp

  1. #include "vehicle_pose_and_velocity_updater.h"
  2.  
  3. int main(int argc, char** argv){
  4.   ros::init(argc, argv, "vehicle_pose_and_velocity_updater");
  5.   vehicle_pose_and_velocity_updater vehicle_pose_and_velocity_updater;
  6.   vehicle_pose_and_velocity_updater.control();
  7.   return 0;
  8. }
  9.  
  10. void vehicle_pose_and_velocity_updater::model_states_callback(const gazebo_msgs::ModelStatesConstPtr &states){
  11.   int modelCount = states->name.size();
  12.   for(int modelInd = 0; modelInd < modelCount; ++modelInd)
  13.   {
  14.     if(states->name[modelInd] == "smart")
  15.     {
  16.       vehicle_position = states->pose[modelInd];
  17.       vehicle_velocity = states->twist[modelInd];
  18.       orientation = vehicle_position.orientation;
  19.  
  20.       tf::Quaternion quaternion1(
  21.         orientation.x,
  22.         orientation.y,
  23.         orientation.z,
  24.         orientation.w
  25.       );
  26.       double roll, pitch, yaw;//定义存储r\p\y的容器
  27.       tf::Matrix3x3(quaternion1).getRPY(roll, pitch, yaw);//进行转换
  28.       time_stamp = ros::Time::now();
  29.  
  30.       geometry_msgs::PoseStamped center_pose;
  31.       center_pose.header.frame_id = "/world";
  32.       center_pose.header.stamp = time_stamp;
  33.       center_pose.pose.position = vehicle_position.position;
  34. 		  center_pose.pose.orientation = vehicle_position.orientation;
  35. 		  center_pose_pub.publish(center_pose);
  36.  
  37.       geometry_msgs::PoseStamped rear_pose;
  38.       rear_pose.header.frame_id = "/world";
  39.       rear_pose.header.stamp = time_stamp;
  40.       double center_x = vehicle_position.position.x;
  41.       double center_y = vehicle_position.position.y;
  42.       double rear_x = center_x - cos(yaw) * 0.945;
  43.       double rear_y = center_y - sin(yaw) * 0.945;
  44.       rear_pose.pose.position.x = rear_x;
  45.       rear_pose.pose.position.y = rear_y;
  46.       rear_pose.pose.orientation = vehicle_position.orientation;
  47.       rear_pose_pub.publish(rear_pose);
  48.  
  49.       geometry_msgs::TwistStamped velocity;
  50.       velocity.header.frame_id ="world";
  51.       velocity.header.stamp = time_stamp;
  52.       velocity.twist.linear = vehicle_velocity.linear;
  53.       velocity.twist.angular = vehicle_velocity.angular;
  54.       vel_pub.publish(velocity);
  55.     }
  56.   }
  57. }
  58.  
  59. void vehicle_pose_and_velocity_updater::control(){
  60.   ros::spin();
  61. }
  62.  
  63.  
  64. vehicle_pose_and_velocity_updater::vehicle_pose_and_velocity_updater(){
  65.   rear_pose_pub = n.advertise<geometry_msgs::PoseStamped>("/smart/rear_pose", 10);
  66.   center_pose_pub = n.advertise<geometry_msgs::PoseStamped>("/smart/center_pose", 10);
  67.   vel_pub = n.advertise<geometry_msgs::TwistStamped>("/smart/velocity",10);
  68.  
  69.   model_states_sub = n.subscribe("/gazebo/model_states", 10, &vehicle_pose_and_velocity_updater::model_states_callback,this);
  70. }
  71.  
  72. vehicle_pose_and_velocity_updater::~vehicle_pose_and_velocity_updater(){
  73.  
  74. }

waypoint_loader.cpp

  1. #include "waypoint_loader.h"
  2.  
  3. double waypoint_loader::two_points_distance(const geometry_msgs::Point &current_position, const geometry_msgs::Point &last_position){
  4.   double x,y,z;
  5.   x = current_position.x - last_position.x;
  6.   y = current_position.y - last_position.y;
  7.   z = current_position.z - last_position.z;
  8.   return sqrt(x*x+y*y+z*z);
  9. }
  10.  
  11. vector<styx_msgs::Waypoint> waypoint_loader::decelerate(const vector<styx_msgs::Waypoint> &data){
  12.   vector<styx_msgs::Waypoint> Return_data = data;
  13.   styx_msgs::Waypoint last;
  14.   double dist, vel;
  15.   last = data[waypoint_count -1];
  16.   last.twist.twist.linear.x = 0;
  17.   for (int i = 0; i < data.size(); i++)
  18.   {
  19.     dist = two_points_distance(data[i].pose.pose.position, last.pose.pose.position);
  20.     vel = sqrt(2* MAX_DECEL* dist);
  21.     if(vel<1){
  22.       vel=0;
  23.     }
  24.     Return_data[i].twist.twist.linear.x = min(vel, data[i].twist.twist.linear.x);
  25.   }
  26.   return Return_data;
  27. }
  28.  
  29.  
  30. void waypoint_loader::getPose(std::string s, double *v)
  31. {
  32.   int p = 0;
  33.   int q = 0;
  34.   for (int i = 0; i < s.size(); i++)
  35.   {
  36.     if (s[i] == ',' || i == s.size() - 1)
  37.     {
  38.       char tab2[16];
  39.       strcpy(tab2, s.substr(p, i - p).c_str());
  40.       v[q] = std::strtod(tab2, NULL);
  41.       p = i + 1;
  42.       q++;
  43.     }
  44.   }
  45. }
  46.  
  47. int waypoint_loader::process(){
  48.   std::ifstream f(ros::package::getPath("waypoint_loader") + "/waypoints/" + "waypoints.csv");
  49.   //f.open(ros::package::find(plan_pkg)+"/paths/path.csv"); //ros::package::find(plan_pkg)
  50.   if (!f.is_open())
  51.   {
  52.     ROS_ERROR("failed to open file");
  53.     return 0;
  54.   }
  55.   std::string line;
  56.   std::vector<styx_msgs::Waypoint> waypoints;
  57.   styx_msgs::Lane lane;
  58.   double scale = 100;
  59.   int count = -1;
  60.   nav_msgs::Path ros_path_;
  61.  
  62.   geometry_msgs::PoseArray track;
  63.   track.header.stamp = ros::Time::now();
  64.   track.header.frame_id = "/world";
  65.   while (std::getline(f, line))
  66.   {
  67.     count++;
  68.     double pose[3];
  69.     getPose(line, pose);
  70.     tf::Quaternion q = tf::createQuaternionFromRPY(0, 0, pose[2]);
  71.     geometry_msgs::Pose pose1;
  72.  
  73.     geometry_msgs::PoseStamped pose2;
  74.     styx_msgs::Waypoint pose3;
  75.     ros_path_.header.frame_id = "/world";
  76.     ros_path_.header.stamp = ros::Time::now();  
  77.     pose2.header = ros_path_.header;
  78.  
  79.     pose2.pose.position.x = pose[0];
  80.     pose2.pose.position.y = pose[1];
  81.     pose2.pose.position.z = 0;
  82.  
  83.     pose1.orientation.x = q.x();
  84.     pose1.orientation.y = q.y();
  85.     pose1.orientation.z = q.z();
  86.     pose1.orientation.w = q.w();
  87.     pose1.position.x = pose[0];
  88.     pose1.position.y = pose[1];
  89.     pose1.position.z = 0;
  90.  
  91.     pose3.pose.pose.position.x = pose[0];
  92.     pose3.pose.pose.position.y = pose[1];
  93.     pose3.pose.pose.position.z = 0;
  94.     pose3.pose.pose.orientation.x = q.x();
  95.     pose3.pose.pose.orientation.y = q.y();
  96.     pose3.pose.pose.orientation.z = q.z();
  97.     pose3.pose.pose.orientation.w = q.w();
  98.     pose3.twist.twist.linear.x = 2.78;
  99.     pose3.forward = true;
  100.  
  101.     waypoints.push_back(pose3);
  102.     track.poses.push_back(pose1);
  103.     ros_path_.poses.push_back(pose2);
  104.   }
  105.  
  106.   waypoint_count = waypoints.size();
  107.   waypoints = decelerate(waypoints);
  108.  
  109.   lane.header.frame_id = "world";
  110.   lane.header.stamp = ros::Time::now();
  111.   lane.waypoints = waypoints;
  112.  
  113.   while (ros::ok())
  114.   {
  115.     path_pub.publish(track);
  116.     state_pub_.publish(ros_path_);
  117.     path.publish(lane);
  118.   }
  119.   return 0;
  120. }
  121.  
  122. int main(int argc, char **argv)
  123. {
  124.   setlocale(LC_ALL, "");
  125.   ros::init(argc, argv, "waypoint_loader");
  126.   waypoint_loader waypoint_loader;
  127.   ros::AsyncSpinner spinner(1);
  128.   spinner.start();
  129.   sleep(1);
  130.   waypoint_loader.process();
  131. }
  132.

waypoint_updater.cpp

  1. #include "waypoint_updater.h"
  2.  
  3. void waypoint_updater::publish_waypoints(const int &idx, const ros::Publisher &final_waypoints_pub, const ros::Publisher &final_path_pub){
  4.   styx_msgs::Lane Lane_pub;
  5.   Lane_pub.header = base_waypoints.header;
  6.   if(idx + LOOKAHEAD_WPS < waypoint_count){
  7.     for (int i = 0; i < LOOKAHEAD_WPS; i++)
  8.     {
  9.     Lane_pub.waypoints.push_back(base_waypoints.waypoints[idx+i]);
  10.     }
  11.   }
  12.   else
  13.   {
  14.     for (int i = 0; i < waypoint_count - idx; i++)
  15.     {
  16.     Lane_pub.waypoints.push_back(base_waypoints.waypoints[idx+i]);
  17.     }
  18.   }
  19.  
  20.   nav_msgs::Path path;
  21.   path.header.frame_id = "/world";
  22.   for(int i = 0; i < Lane_pub.waypoints.size();i++){
  23.     geometry_msgs::PoseStamped path_element;
  24.     path_element.pose.position.x = Lane_pub.waypoints[i].pose.pose.position.x;
  25.     path_element.pose.position.y = Lane_pub.waypoints[i].pose.pose.position.y;
  26.     path_element.pose.position.z = Lane_pub.waypoints[i].pose.pose.position.z;
  27.     path.poses.push_back(path_element);
  28.   }
  29.   final_waypoints_pub.publish(Lane_pub);
  30.   final_path_pub.publish(path);
  31. }
  32.  
  33. int waypoint_updater::get_closest_waypoint_idx(){
  34.   pcl::PointXYZ searchPoint;
  35.   searchPoint.x = pose.pose.position.x;
  36.   searchPoint.y = pose.pose.position.y;
  37.   searchPoint.z = pose.pose.position.z;
  38.  
  39.   vector<int> pointIdxNKNSearch(K);
  40.   vector<float> pointNKNSquaredDistance(K);
  41.   if(kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0){
  42.     for (size_t i=0; i<pointIdxNKNSearch.size (); ++i){
  43.       // cout << "    点" << (*cloud)[pointIdxNKNSearch[i]].x
  44.       //   << " " << (*cloud)[pointIdxNKNSearch[i]].y
  45.       //   << " " << (*cloud)[pointIdxNKNSearch[i]].z
  46.       //   << " (平方距离: " << pointNKNSquaredDistance[i] << ")" << endl;
  47.       // cout << "Index :" << pointIdxNKNSearch[i] << endl;
  48.     }
  49.     return pointIdxNKNSearch[0];
  50.   }
  51. }
  52.  
  53. void waypoint_updater::pose_cb(const geometry_msgs::PoseStamped &msg)
  54. {
  55.   pose = msg;
  56.   // cout << pose << endl;
  57. }
  58.  
  59. void waypoint_updater::waypoint_cb(const styx_msgs::Lane &waypoints)
  60. {
  61.   waypoint_count = waypoints.waypoints.size();
  62.   base_waypoints = waypoints;
  63.   if((*cloud).size() == 0 ){
  64.     cloud->width = 56;
  65.     cloud->height = 1; 
  66.     cloud->points.resize(cloud->width * cloud->height); 
  67.     for (int i = 0; i < waypoints.waypoints.size(); i++){
  68.       (*cloud)[i].x = waypoints.waypoints[i].pose.pose.position.x;
  69.       (*cloud)[i].y = waypoints.waypoints[i].pose.pose.position.y;
  70.       (*cloud)[i].z = waypoints.waypoints[i].pose.pose.position.z;
  71.     }
  72.     kdtree.setInputCloud(cloud);
  73.   }
  74. }
  75.  
  76. void waypoint_updater::process(){
  77.   while(ros::ok())
  78.   {
  79.     ros::spinOnce();
  80.     if(base_waypoints.waypoints.size()!=0){
  81.       int closest_waypoint_idx = get_closest_waypoint_idx();
  82.       publish_waypoints(closest_waypoint_idx, final_waypoints_pub, final_path_pub);
  83.     }
  84.   }
  85. }
  86.  
  87. int main(int argc, char** argv){
  88.   ros::init(argc, argv, "waypoint_updater");
  89.   waypoint_updater waypoint_updater;
  90.   waypoint_updater.process();
  91.   return 0;
  92. }

pure_persuit.cpp

  1. #include "pure_persuit.h"
  2.  
  3. geometry_msgs::Twist pure_persuit::calculateTwistCommand(){
  4.   double lad = 0.0;
  5.   int targetIndex = currentWaypoints.waypoints.size() -1;
  6.   for(int i = 0; i<currentWaypoints.waypoints.size(); i++){
  7.     if(i+1 < currentWaypoints.waypoints.size()){
  8.       double this_x = currentWaypoints.waypoints[i].pose.pose.position.x;
  9. 			double this_y = currentWaypoints.waypoints[i].pose.pose.position.y;
  10. 			double next_x = currentWaypoints.waypoints[i+1].pose.pose.position.x;
  11. 			double next_y = currentWaypoints.waypoints[i+1].pose.pose.position.y;
  12.       lad = lad + hypot(next_x - this_x, next_y - this_y);
  13.       if(lad > HORIZON){
  14.         targetIndex = i+1;
  15.         break;
  16.       }
  17.     }
  18.   }
  19.  
  20.   geometry_msgs::Twist twistCmd;
  21.   styx_msgs::Waypoint targetWaypoint;
  22.   double targetSpeed, targetX, targetY, currentX, currentY;
  23.   targetWaypoint = currentWaypoints.waypoints[targetIndex];
  24.   targetSpeed = currentWaypoints.waypoints[0].twist.twist.linear.x;
  25.   targetX = targetWaypoint.pose.pose.position.x;
  26.   targetY = targetWaypoint.pose.pose.position.y;
  27.   currentX = currentPose.pose.position.x;
  28.   currentY = currentPose.pose.position.y;
  29.   tf::Quaternion q(currentPose.pose.orientation.x, currentPose.pose.orientation.y, currentPose.pose.orientation.z, currentPose.pose.orientation.w);
  30.   double roll, pitch, yaw;
  31.   tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
  32.   auto alpha = atan2(targetY - currentY, targetX - currentX) - yaw;
  33.   auto l = sqrt(pow(currentX - targetX, 2) + pow(currentY - targetY, 2));
  34.   if(l > 0.5){
  35.     auto theta = atan(2 * 1.868 * sin(alpha) / l);
  36.     twistCmd.linear.x = targetSpeed;
  37.     twistCmd.angular.z = theta; 
  38.   }
  39.   else{
  40.     twistCmd.linear.x = 0;
  41.     twistCmd.angular.z = 0; 
  42.   }
  43.   return twistCmd;
  44. }
  45.  
  46. void pure_persuit::process(){
  47.   while (ros::ok())
  48.   {
  49.     ros::spinOnce();
  50.     if(currentWaypoints.waypoints.size()!=0){
  51.     geometry_msgs::Twist twistCommand = calculateTwistCommand();
  52.     cmd_vel.publish(twistCommand);
  53.     }
  54.   }
  55. }
  56.  
  57. void pure_persuit::pose_cb(const geometry_msgs::PoseStamped &data1){
  58.   currentPose = data1;
  59. }
  60.  
  61. void pure_persuit::vel_cb(const geometry_msgs::TwistStamped &data2){
  62.   currentVelocity = data2;
  63. }
  64.  
  65. void pure_persuit::lane_cb(const styx_msgs::Lane &data3){
  66.   currentWaypoints = data3;
  67. }
  68.  
  69. int main(int argc, char** argv){
  70.   ros::init(argc, argv, "pure_persuit");
  71.   pure_persuit pure_persuit;
  72.   pure_persuit.process();
  73.   return 0;
  74. }

github：GitHub - casso1993/Pure_Persuit_Simulation

1.open gazebo

roslaunch gazebo_ros empty_world.launch

2.run the set environment

roslaunch car_model spawn_car.launch

3.run rviz

rviz

File -> open config -> "src/car_model/rviz_config/samrt.rviz"

4.run pure persuit

roslaunch pure_persuit pure_persuit.launch

偷懒，先不写原理

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/weixin_40725706/article/detail/592102
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号