概要

当你只有一个相机的时候，想要快速实现机器人跟随功能，没有里程计的情况下，就可以看这里了。这篇博文实现了一个无里程计下的纯跟踪算法。

生成相机坐标系下的三维坐标

订阅相机内参

void PoseRecognition::info_rgb_callback(const sensor_msgs::CameraInfo &caminfo_rgb){if (colorInfo)return;else{std::cout<< "\n color intrinsics: " << std::endl;innerTransformationColor<<caminfo_rgb.K.at(0), caminfo_rgb.K.at(1), caminfo_rgb.K.at(2), caminfo_rgb.K.at(3),caminfo_rgb.K.at(4), caminfo_rgb.K.at(5),caminfo_rgb.K.at(6), caminfo_rgb.K.at(7),caminfo_rgb.K.at(8);for(int i = 0; i<3; i++){for (int j = 0; j < 3; ++j){std::cout<<innerTransformationColor(i, j) << "\t";}std::cout << std::endl;}colorInfo = true;}
}

像素坐标转深度坐标

Eigen::Vector3f ImagePixObj;
ImagePixObj << centerX, centerY , 1.00;
Eigen::Vector3f CameraPixObj = innerTransformationColor.inverse() * ImagePixObj * depth;

无里程计下的纯跟踪算法实现

头文件

#include <geometry_msgs/Twist.h>
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>#include <algorithm>
#include <cassert>
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include <signal.h>
#include <numeric>#include "geometry_msgs/PoseStamped.h"
#include "nav_msgs/Odometry.h"
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"class PurePursuitController
{public:PurePursuitController();void poseCallback(const nav_msgs::Odometry &currentWaypoint);private:ros::NodeHandle n, nPrivate;ros::Publisher purepersuit_;ros::Publisher path_pub_, referencePathPub, controlPub, markerPub;ros::Subscriber splinePath, carVel, carPose, subOrder, subStatus;nav_msgs::Path path;std::string topicTrajectory, topicCmdVel, topicCurrentPose, filePath, topicOrder, topicGPSStatus;float Ld;   //轴距float PREVIEW_DIS;  //预瞄距离float carVelocity = 0;float preview_dis = 0;float k = 0.1;int pointNum;  //保存路径点的个数int startIdx; //开始循迹的下标int beginIdx; //trajectory begin idxbool initTrajectory = false;bool directionMatch = false;float midDistance; // 中值距离大小float maxDis; //最大值距离float linearVel; //线速度bool isReverse;  //是否轨迹反转bool isStopCar; //是否停车(停障)bool isStopCarCmd; //是否停车（平台）int patientCount; //重置前的耐心次数/*create kalman filter*/float lastPosX, lastPosY;
};

主函数

#include "pure_pursuit_image/pure_pursuit.h"using namespace std;PurePursuitController::PurePursuitController():nPrivate("~")
{/*params setting*/nPrivate.param("Ld", Ld, float(1.0));nPrivate.param("PREVIEW_DIS", PREVIEW_DIS, float(2.0));nPrivate.param("linearVel", linearVel, float(0.3));nPrivate.param("isReverse", isReverse, bool(false));nPrivate.param("topicCurrentPose", topicCurrentPose, std::string("/objectTracking/odom"));nPrivate.param("topicCmdVel", topicCmdVel, std::string("/cmd_vel"));/*订阅话题*/carPose = n.subscribe(topicCurrentPose.c_str(), 1000, &PurePursuitController::poseCallback, this);//创建Publisher，发送经过pure_pursuit计算后的转角及速度purepersuit_ = n.advertise<geometry_msgs::Twist>(topicCmdVel.c_str(), 10);ROS_INFO("Pure pursuit init successfully!!!");
}//计算发送给模型车的转角
void PurePursuitController::poseCallback(const nav_msgs::Odometry &currentWaypoint) {// ROS_INFO("poseCallback");auto currentPositionX = currentWaypoint.pose.pose.position.x;auto currentPositionY = currentWaypoint.pose.pose.position.y;auto currentPositionZ = 0.0;auto currentQuaternionX = currentWaypoint.pose.pose.orientation.x;auto currentQuaternionY = currentWaypoint.pose.pose.orientation.y;auto currentQuaternionZ = currentWaypoint.pose.pose.orientation.z;auto currentQuaternionW = currentWaypoint.pose.pose.orientation.w;auto currentPositionYaw = tf::getYaw(currentWaypoint.pose.pose.orientation);/*判断是否到达终点*/float cur2endDistance = sqrt(pow(currentPositionY, 2) + pow(currentPositionX, 2));if(cur2endDistance < 2*Ld){ROS_INFO("The car has reached the end!!!");geometry_msgs::Twist vel_msg;vel_msg.linear.x = 0;vel_msg.angular.z = 0;purepersuit_.publish(vel_msg);}// ROS_INFO("The rate of progress is %d / %d. ", startIdx, pointNum);float alpha = atan2(currentPositionY, currentPositionX) - currentPositionYaw;// 当前点和目标点的距离Idfloat dl = sqrt(pow(currentPositionY, 2) + pow(currentPositionX, 2));/*原地转向，使得车的航向角对齐轨迹方向*/float theta = atan(2 * Ld * sin(alpha) / dl);if (dl > 2*Ld ) {ROS_INFO("Moving the car");// double delta = atan2(2* Ld * sin(alpha), preview_dis);geometry_msgs::Twist vel_msg;vel_msg.linear.x = linearVel;vel_msg.angular.z = theta;purepersuit_.publish(vel_msg);} else {geometry_msgs::Twist vel_msg;vel_msg.linear.x = 0;vel_msg.angular.z = 0;purepersuit_.publish(vel_msg);}
}int main(int argc, char **argv) {//创建节点ros::init(argc, argv, "pure_pursuit_image");PurePursuitController ppc;  ros::spin();return 0;
}