运动控制入门到入土:pure persuit纯追踪算法gazebo仿真

加葱casso

已于 2023-07-02 22:47:45 修改

阅读量434

点赞数

文章标签：算法机器人

于 2023-07-02 22:17:45 首次发布

本文链接：https://blog.csdn.net/bingshuiyuyi/article/details/131506127

版权

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

感谢B站up Imjusty的教学

cmdvel2gazebo.cpp

#include "cmdvel2gazebo.h"

int main(int argc, char** argv){
  ros::init(argc, argv, "cmdvel2gazebo");
  cmdvel2gazebo cmdvel2gazebo;
  cmdvel2gazebo.control();
  return 0;
}

void cmdvel2gazebo::Cmd_Vel_Callback(const geometry_msgs::Twist &twist){
  velocity = twist.linear.x / 0.3;
  // cout << "velocity:" << velocity <<endl;
  angle = max(-maxsteer, min(maxsteer, twist.angular.z));
  // cout << "angle:" << angle <<endl;
  last_Time = ros::Time::now();
  publish();
}

void cmdvel2gazebo::control(){
  ros::spin();
}

void cmdvel2gazebo::publish(){
  std_msgs::Float64 msg_vel_L, msg_vel_R, msg_ang_L, msg_ang_R;
  delta_Time = (ros::Time::now()- last_Time).toSec();
  msgs_too_old = delta_Time > timeout ? 1: 0;
  if(msgs_too_old){
    velocity = 0;
    angle= 0;
    msg_vel_L.data = velocity;
    msg_vel_R.data = velocity;
    msg_ang_L.data = angle;
    msg_ang_R.data = angle;

    pub_rearL.publish(msg_vel_L);
    pub_rearR.publish(msg_vel_R);
    pub_steerL.publish(msg_ang_L);
    pub_steerR.publish(msg_ang_R);
  }

  // cout << "velocity1:" << velocity <<endl;
  // cout << "angle1:" << angle <<endl;

  if(angle!= 0){
    radius = wheelbase/fabs(tan(angle));
    radius_rearL = radius- (copysign(1, angle)* (Tread/ 2.0));
    radius_rearR = radius+ (copysign(1, angle)* (Tread/ 2.0));
    radius_frontL = radius- (copysign(1, angle)* (Tread/ 2.0));
    radius_frontR = radius+ (copysign(1, angle)* (Tread/ 2.0));

    velocity_rearL = velocity* radius_rearL/ radius;
    velocity_rearR = velocity* radius_rearR/ radius;

    msg_vel_L.data = velocity_rearL;
    msg_vel_R.data = velocity_rearR;


    pub_rearL.publish(msg_vel_L);
    pub_rearR.publish(msg_vel_R);

    angle_frontL = atan2(wheelbase, radius_frontL)* copysign(1, angle);
    angle_frontR = atan2(wheelbase, radius_frontR)* copysign(1, angle);

    // cout << "angle_L:" << angle_frontL <<endl;
    // cout << "angle_R:" << angle_frontR <<endl;

    msg_ang_L.data = angle_frontL;
    msg_ang_R.data = angle_frontR;

    pub_steerL.publish(msg_ang_L);
    pub_steerR.publish(msg_ang_R);
  }
  else{
    msg_vel_L.data = velocity;
    msg_vel_R.data = velocity;
    msg_ang_L.data = angle;
    msg_ang_R.data = angle;

    pub_rearL.publish(msg_vel_L);
    pub_rearR.publish(msg_vel_R);
    pub_steerL.publish(msg_ang_L);
    pub_steerR.publish(msg_ang_R);
  }

}

cmdvel2gazebo::cmdvel2gazebo()
{
  pub_steerL = n.advertise <std_msgs::Float64>("/smart/front_left_steering_position_controller/command", 10);
  pub_steerR = n.advertise <std_msgs::Float64>("/smart/front_right_steering_position_controller/command", 10);
  pub_rearL = n.advertise <std_msgs::Float64>("/smart/rear_left_velocity_controller/command",10);
  pub_rearR = n.advertise <std_msgs::Float64>("/smart/rear_right_velocity_controller/command",10);
  cmd_vel_sub = n.subscribe("/smart/cmd_vel", 100, &cmdvel2gazebo::Cmd_Vel_Callback, this);


  last_Time= ros::Time::now();


  rMax = wheelbase/ tan(maxsteerInside);
  rIdeal = rMax+ (Tread/ 2.0);
  maxsteer = atan2(wheelbase, rIdeal);
}

cmdvel2gazebo::~cmdvel2gazebo()
{
}

transform_publisher.cpp

#include "transform_publisher.h"

int main(int argc, char** argv){
  ros::init(argc, argv, "transform_publisher");
  transform_publisher transform_publisher;
  transform_publisher.control();
}

void transform_publisher::pose_callback(const geometry_msgs::PoseStamped &pose_msgs){
  auto pose = pose_msgs.pose.position;
  cout << pose << endl;
  auto orientation = pose_msgs.pose.orientation;
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin(tf::Vector3(pose.x, pose.y, pose.z));
  transform.setRotation(tf::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w));

  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "base_link"));
}

void transform_publisher::control(){
  ros::spin();
}

transform_publisher::transform_publisher(){
  center_pose_sub = n.subscribe("/smart/center_pose", 10, &transform_publisher::pose_callback, this);
}

transform_publisher::~transform_publisher(){

}

vehicle_pose_and_velocity_updater.cpp

#include "vehicle_pose_and_velocity_updater.h"

int main(int argc, char** argv){
  ros::init(argc, argv, "vehicle_pose_and_velocity_updater");
  vehicle_pose_and_velocity_updater vehicle_pose_and_velocity_updater;
  vehicle_pose_and_velocity_updater.control();
  return 0;
}

void vehicle_pose_and_velocity_updater::model_states_callback(const gazebo_msgs::ModelStatesConstPtr &states){
  int modelCount = states->name.size();
  for(int modelInd = 0; modelInd < modelCount; ++modelInd)
  {
    if(states->name[modelInd] == "smart")
    {
      vehicle_position = states->pose[modelInd];
      vehicle_velocity = states->twist[modelInd];
      orientation = vehicle_position.orientation;

      tf::Quaternion quaternion1(
        orientation.x,
        orientation.y,
        orientation.z,
        orientation.w
      );
      double roll, pitch, yaw;//定义存储r\p\y的容器
      tf::Matrix3x3(quaternion1).getRPY(roll, pitch, yaw);//进行转换
      time_stamp = ros::Time::now();

      geometry_msgs::PoseStamped center_pose;
      center_pose.header.frame_id = "/world";
      center_pose.header.stamp = time_stamp;
      center_pose.pose.position = vehicle_position.position;
		  center_pose.pose.orientation = vehicle_position.orientation;
		  center_pose_pub.publish(center_pose);

      geometry_msgs::PoseStamped rear_pose;
      rear_pose.header.frame_id = "/world";
      rear_pose.header.stamp = time_stamp;
      double center_x = vehicle_position.position.x;
      double center_y = vehicle_position.position.y;
      double rear_x = center_x - cos(yaw) * 0.945;
      double rear_y = center_y - sin(yaw) * 0.945;
      rear_pose.pose.position.x = rear_x;
      rear_pose.pose.position.y = rear_y;
      rear_pose.pose.orientation = vehicle_position.orientation;
      rear_pose_pub.publish(rear_pose);

      geometry_msgs::TwistStamped velocity;
      velocity.header.frame_id ="world";
      velocity.header.stamp = time_stamp;
      velocity.twist.linear = vehicle_velocity.linear;
      velocity.twist.angular = vehicle_velocity.angular;
      vel_pub.publish(velocity);
    }
  }
}

void vehicle_pose_and_velocity_updater::control(){
  ros::spin();
}


vehicle_pose_and_velocity_updater::vehicle_pose_and_velocity_updater(){
  rear_pose_pub = n.advertise<geometry_msgs::PoseStamped>("/smart/rear_pose", 10);
  center_pose_pub = n.advertise<geometry_msgs::PoseStamped>("/smart/center_pose", 10);
  vel_pub = n.advertise<geometry_msgs::TwistStamped>("/smart/velocity",10);

  model_states_sub = n.subscribe("/gazebo/model_states", 10, &vehicle_pose_and_velocity_updater::model_states_callback,this);
}

vehicle_pose_and_velocity_updater::~vehicle_pose_and_velocity_updater(){

}

waypoint_loader.cpp

#include "waypoint_loader.h"

double waypoint_loader::two_points_distance(const geometry_msgs::Point &current_position, const geometry_msgs::Point &last_position){
  double x,y,z;
  x = current_position.x - last_position.x;
  y = current_position.y - last_position.y;
  z = current_position.z - last_position.z;
  return sqrt(x*x+y*y+z*z);
}

vector<styx_msgs::Waypoint> waypoint_loader::decelerate(const vector<styx_msgs::Waypoint> &data){
  vector<styx_msgs::Waypoint> Return_data = data;
  styx_msgs::Waypoint last;
  double dist, vel;
  last = data[waypoint_count -1];
  last.twist.twist.linear.x = 0;
  for (int i = 0; i < data.size(); i++)
  {
    dist = two_points_distance(data[i].pose.pose.position, last.pose.pose.position);
    vel = sqrt(2* MAX_DECEL* dist);
    if(vel<1){
      vel=0;
    }
    Return_data[i].twist.twist.linear.x = min(vel, data[i].twist.twist.linear.x);
  }
  return Return_data;
}


void waypoint_loader::getPose(std::string s, double *v)
{
  int p = 0;
  int q = 0;
  for (int i = 0; i < s.size(); i++)
  {
    if (s[i] == ',' || i == s.size() - 1)
    {
      char tab2[16];
      strcpy(tab2, s.substr(p, i - p).c_str());
      v[q] = std::strtod(tab2, NULL);
      p = i + 1;
      q++;
    }
  }
}

int waypoint_loader::process(){
  std::ifstream f(ros::package::getPath("waypoint_loader") + "/waypoints/" + "waypoints.csv");
  //f.open(ros::package::find(plan_pkg)+"/paths/path.csv"); //ros::package::find(plan_pkg)
  if (!f.is_open())
  {
    ROS_ERROR("failed to open file");
    return 0;
  }
  std::string line;
  std::vector<styx_msgs::Waypoint> waypoints;
  styx_msgs::Lane lane;
  double scale = 100;
  int count = -1;
  nav_msgs::Path ros_path_;

  geometry_msgs::PoseArray track;
  track.header.stamp = ros::Time::now();
  track.header.frame_id = "/world";
  while (std::getline(f, line))
  {
    count++;
    double pose[3];
    getPose(line, pose);
    tf::Quaternion q = tf::createQuaternionFromRPY(0, 0, pose[2]);
    geometry_msgs::Pose pose1;

    geometry_msgs::PoseStamped pose2;
    styx_msgs::Waypoint pose3;
    ros_path_.header.frame_id = "/world";
    ros_path_.header.stamp = ros::Time::now();  
    pose2.header = ros_path_.header;

    pose2.pose.position.x = pose[0];
    pose2.pose.position.y = pose[1];
    pose2.pose.position.z = 0;

    pose1.orientation.x = q.x();
    pose1.orientation.y = q.y();
    pose1.orientation.z = q.z();
    pose1.orientation.w = q.w();
    pose1.position.x = pose[0];
    pose1.position.y = pose[1];
    pose1.position.z = 0;

    pose3.pose.pose.position.x = pose[0];
    pose3.pose.pose.position.y = pose[1];
    pose3.pose.pose.position.z = 0;
    pose3.pose.pose.orientation.x = q.x();
    pose3.pose.pose.orientation.y = q.y();
    pose3.pose.pose.orientation.z = q.z();
    pose3.pose.pose.orientation.w = q.w();
    pose3.twist.twist.linear.x = 2.78;
    pose3.forward = true;

    waypoints.push_back(pose3);
    track.poses.push_back(pose1);
    ros_path_.poses.push_back(pose2);
  }

  waypoint_count = waypoints.size();
  waypoints = decelerate(waypoints);

  lane.header.frame_id = "world";
  lane.header.stamp = ros::Time::now();
  lane.waypoints = waypoints;

  while (ros::ok())
  {
    path_pub.publish(track);
    state_pub_.publish(ros_path_);
    path.publish(lane);
  }
  return 0;
}

int main(int argc, char **argv)
{
  setlocale(LC_ALL, "");
  ros::init(argc, argv, "waypoint_loader");
  waypoint_loader waypoint_loader;
  ros::AsyncSpinner spinner(1);
  spinner.start();
  sleep(1);
  waypoint_loader.process();
}

waypoint_updater.cpp

#include "waypoint_updater.h"

void waypoint_updater::publish_waypoints(const int &idx, const ros::Publisher &final_waypoints_pub, const ros::Publisher &final_path_pub){
  styx_msgs::Lane Lane_pub;
  Lane_pub.header = base_waypoints.header;
  if(idx + LOOKAHEAD_WPS < waypoint_count){
    for (int i = 0; i < LOOKAHEAD_WPS; i++)
    {
    Lane_pub.waypoints.push_back(base_waypoints.waypoints[idx+i]);
    }
  }
  else
  {
    for (int i = 0; i < waypoint_count - idx; i++)
    {
    Lane_pub.waypoints.push_back(base_waypoints.waypoints[idx+i]);
    }
  }

  nav_msgs::Path path;
  path.header.frame_id = "/world";
  for(int i = 0; i < Lane_pub.waypoints.size();i++){
    geometry_msgs::PoseStamped path_element;
    path_element.pose.position.x = Lane_pub.waypoints[i].pose.pose.position.x;
    path_element.pose.position.y = Lane_pub.waypoints[i].pose.pose.position.y;
    path_element.pose.position.z = Lane_pub.waypoints[i].pose.pose.position.z;
    path.poses.push_back(path_element);
  }
  final_waypoints_pub.publish(Lane_pub);
  final_path_pub.publish(path);
}

int waypoint_updater::get_closest_waypoint_idx(){
  pcl::PointXYZ searchPoint;
  searchPoint.x = pose.pose.position.x;
  searchPoint.y = pose.pose.position.y;
  searchPoint.z = pose.pose.position.z;

  vector<int> pointIdxNKNSearch(K);
  vector<float> pointNKNSquaredDistance(K);
  if(kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0){
    for (size_t i=0; i<pointIdxNKNSearch.size (); ++i){
      // cout << "    点" << (*cloud)[pointIdxNKNSearch[i]].x
      //   << " " << (*cloud)[pointIdxNKNSearch[i]].y
      //   << " " << (*cloud)[pointIdxNKNSearch[i]].z
      //   << " (平方距离: " << pointNKNSquaredDistance[i] << ")" << endl;
      // cout << "Index :" << pointIdxNKNSearch[i] << endl;
    }
    return pointIdxNKNSearch[0];
  }
}

void waypoint_updater::pose_cb(const geometry_msgs::PoseStamped &msg)
{
  pose = msg;
  // cout << pose << endl;
}

void waypoint_updater::waypoint_cb(const styx_msgs::Lane &waypoints)
{
  waypoint_count = waypoints.waypoints.size();
  base_waypoints = waypoints;
  if((*cloud).size() == 0 ){
    cloud->width = 56;
    cloud->height = 1; 
    cloud->points.resize(cloud->width * cloud->height); 
    for (int i = 0; i < waypoints.waypoints.size(); i++){
      (*cloud)[i].x = waypoints.waypoints[i].pose.pose.position.x;
      (*cloud)[i].y = waypoints.waypoints[i].pose.pose.position.y;
      (*cloud)[i].z = waypoints.waypoints[i].pose.pose.position.z;
    }
    kdtree.setInputCloud(cloud);
  }
}

void waypoint_updater::process(){
  while(ros::ok())
  {
    ros::spinOnce();
    if(base_waypoints.waypoints.size()!=0){
      int closest_waypoint_idx = get_closest_waypoint_idx();
      publish_waypoints(closest_waypoint_idx, final_waypoints_pub, final_path_pub);
    }
  }
}

int main(int argc, char** argv){
  ros::init(argc, argv, "waypoint_updater");
  waypoint_updater waypoint_updater;
  waypoint_updater.process();
  return 0;
}

pure_persuit.cpp

#include "pure_persuit.h"

geometry_msgs::Twist pure_persuit::calculateTwistCommand(){
  double lad = 0.0;
  int targetIndex = currentWaypoints.waypoints.size() -1;
  for(int i = 0; i<currentWaypoints.waypoints.size(); i++){
    if(i+1 < currentWaypoints.waypoints.size()){
      double this_x = currentWaypoints.waypoints[i].pose.pose.position.x;
			double this_y = currentWaypoints.waypoints[i].pose.pose.position.y;
			double next_x = currentWaypoints.waypoints[i+1].pose.pose.position.x;
			double next_y = currentWaypoints.waypoints[i+1].pose.pose.position.y;
      lad = lad + hypot(next_x - this_x, next_y - this_y);
      if(lad > HORIZON){
        targetIndex = i+1;
        break;
      }
    }
  }

  geometry_msgs::Twist twistCmd;
  styx_msgs::Waypoint targetWaypoint;
  double targetSpeed, targetX, targetY, currentX, currentY;
  targetWaypoint = currentWaypoints.waypoints[targetIndex];
  targetSpeed = currentWaypoints.waypoints[0].twist.twist.linear.x;
  targetX = targetWaypoint.pose.pose.position.x;
  targetY = targetWaypoint.pose.pose.position.y;
  currentX = currentPose.pose.position.x;
  currentY = currentPose.pose.position.y;
  tf::Quaternion q(currentPose.pose.orientation.x, currentPose.pose.orientation.y, currentPose.pose.orientation.z, currentPose.pose.orientation.w);
  double roll, pitch, yaw;
  tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
  auto alpha = atan2(targetY - currentY, targetX - currentX) - yaw;
  auto l = sqrt(pow(currentX - targetX, 2) + pow(currentY - targetY, 2));
  if(l > 0.5){
    auto theta = atan(2 * 1.868 * sin(alpha) / l);
    twistCmd.linear.x = targetSpeed;
    twistCmd.angular.z = theta; 
  }
  else{
    twistCmd.linear.x = 0;
    twistCmd.angular.z = 0; 
  }
  return twistCmd;
}

void pure_persuit::process(){
  while (ros::ok())
  {
    ros::spinOnce();
    if(currentWaypoints.waypoints.size()!=0){
    geometry_msgs::Twist twistCommand = calculateTwistCommand();
    cmd_vel.publish(twistCommand);
    }
  }
}

void pure_persuit::pose_cb(const geometry_msgs::PoseStamped &data1){
  currentPose = data1;
}

void pure_persuit::vel_cb(const geometry_msgs::TwistStamped &data2){
  currentVelocity = data2;
}

void pure_persuit::lane_cb(const styx_msgs::Lane &data3){
  currentWaypoints = data3;
}

int main(int argc, char** argv){
  ros::init(argc, argv, "pure_persuit");
  pure_persuit pure_persuit;
  pure_persuit.process();
  return 0;
}