项目设计Ⅰ——ROS小车结合aruco码定位到达目标点（源码）

#include <iostream>

#include <fstream>

#include "ros/ros.h"

#include "geometry_msgs/Twist.h"

#include "ar_track_alvar_msgs/AlvarMarkers.h"

#include <openCV2/opencv.hpp>  

#include <tf/transform_datatypes.h>

#include <tf/transform_broadcaster.h>

#include <cmath>

#include<ctime>

//#define M_PI 3.1415926;

using namespace std;

using namespace cv;

   

class SubscribeAndPublish  

{  

    public:

        //aruco码ID

        long long ar_ID;

        //aruco码相对相机位置

        double Xc;

        double Yc;

        double Zc;

        //四元数

        double x;

        double y;

        double z;

        double w;

        //判断

        bool already = false;

        int flag = 0;

        //终点坐标

        double Xd;

        double Yd;

        double distance = 1.5;//终点坐标到aruco码距离,单位m

        double Final_Distance; //小车到终点距离

        double Angle_turn;//欧拉角

        double Omiga;//计算角速度

        double Velocity;//计算线速度

        ros::Time Beginning;

        ros::Duration Interval_time;

        SubscribeAndPublish()  

        {  

            pub_ = n_.advertise<geometry_msgs::Twist>("/cmd_vel", 100);  

           

            sub_ = n_.subscribe("/ar_pose_marker", 100, &SubscribeAndPublish::Callback,this);  

        }  

        void Callback(const ar_track_alvar_msgs::AlvarMarkers::ConstPtr& msg)  

        {  

            geometry_msgs::Twist twist;

            if(msg->markers.size() == 0  and already == false)//未找到aruco码，旋转小车寻找aruco码

            {

                twist.angular.z=0.2;

                pub_.publish(twist);

            }

            else

            {

                already = true;

                twist.angular.z = 0;//找到aruco码，停止小车

                pub_.publish(twist);

           

                //读取aruco码相关信息，包括id，pose，orientation

                ar_ID = msg->markers[0].id;

                Xc = msg->markers[0].pose.pose.position.x;

                Yc = msg->markers[0].pose.pose.position.y;

                Zc = msg->markers[0].pose.pose.position.z;

                x = msg->markers[0].pose.pose.orientation.x;

                y = msg->markers[0].pose.pose.orientation.y;

                z = msg->markers[0].pose.pose.orientation.z;

                w = msg->markers[0].pose.pose.orientation.w;

               

                //使小车正对aruco码

                if(Xc < -0.015)

                {

                    twist.angular.z=0.2;

                    pub_.publish(twist);

                }

                else if(Xc > 0.015)

                {

                    twist.angular.z=-0.2;

                    pub_.publish(twist);

                }

                else

                {

                    flag=1;

                    //通过四元数计算小车与aruco码的偏转角度    

                    tf::Quaternion q(x,y,z,w);

                    tf::Matrix3x3 m(q);

                    double roll, pitch, yaw;

                    m.getRPY(roll, pitch, yaw);

                    double getyaw = yaw*180.0/M_PI;

                   

                    //计算目的地坐标及小车与目的地的距离

                    if(getyaw > 0)

                    {

                        double Anger = M_PI - yaw;

                        Xd = Xc + distance*cos(Anger);

                        Yd = Yc + distance*sin(Anger);

                    }

                    else if(getyaw < 0)

                    {

                        double Anger = M_PI + yaw;

                        Xd = Xc + distance*cos(Anger);

                        Yd = Yc - distance*sin(Anger);

                    }

                    Final_Distance = sqrt(Xd*Xd+Yd*Yd);

                }

                //开始计算角度并转向

                if(flag == 1)

                {

                    Angle_turn = atan2(Yd,Xd);//目的地与小车的世界坐标系方位角

                }

                //逆时针转向目标点

                if(Angle_turn >= 95)

                {

                    Omiga = Angle_turn - M_PI / 2;

                    twist.angular.z = 0.2;

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

                    while(Interval_time.toSec() < Omiga+0.5)

                    {

                        pub_.publish(twist);

                        Interval_time = ros::Time::now() - Beginning;

                    }

                    falg = 2;

                }

                //顺时针转向目标点

                else if(Angle_turn <= 85 )

                {

                    Omiga = Angle_turn - M_PI / 2;

                    twist.angular.z = -0.2;

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

                    while(Interval_time.toSec() < Omiga+0.5)

                    {

                        pub_.publish(twist);

                        Interval_time = ros::Time::now() - Beginning;

                    }

                    falg = 2;

                }

             

            }

            //明确方向后开始往目的地前进

            if (flag == 2)

            {

                twist.linear.x = 0.2;

                Velocity = Final_Distance / 0.2;

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

                while(Interval_time.toSec() < Velocity)

                    {

                        pub_.publish(twist);

                        Interval_time = ros::Time::now() - Beginning;

                    }

                    falg = 3;

            }

            //到达目的地后停止小车，任务结束

            if (flag == 3)

            {

                twist.linear.x = 0;

                twist.angular.z =0;

                pub_.publish(twist);

            }  

        }  

     

    private:  

        ros::NodeHandle n_;  

        ros::Publisher pub_;  

        ros::Subscriber sub_;  

};  

   

int main(int argc, char **argv)  

{  

    //初始化  

    ros::init(argc, argv, "jeremy");  

    //定义对象

    SubscribeAndPublish SAPObject;  

    //调用回调函数

    ros::spin();  

     

    return 0;  

}