#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;

? ? ? ? //四元數(shù)

? ? ? ? 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碼，旋轉(zhuǎn)小車尋找aruco碼

? ? ? ? ? ? {

? ? ? ? ? ? ? ? twist.angular.z=0.2;

? ? ? ? ? ? ? ? pub_.publish(twist);

? ? ? ? ? ? }

? ? ? ? ? ? else

? ? ? ? ? ? {

? ? ? ? ? ? ? ? already = true;

? ? ? ? ? ? ? ? twist.angular.z = 0;//找到aruco碼，停止小車

? ? ? ? ? ? ? ? pub_.publish(twist);

? ? ? ? ? ?

? ? ? ? ? ? ? ? //讀取aruco碼相關(guān)信息，包括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;

? ? ? ? ? ? ? ? ? ? //通過四元數(shù)計算小車與aruco碼的偏轉(zhuǎn)角度 ? ?

? ? ? ? ? ? ? ? ? ? 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);

? ? ? ? ? ? ? ? }

? ? ? ? ? ? ? ? //開始計算角度并轉(zhuǎn)向

? ? ? ? ? ? ? ? if(flag == 1)

? ? ? ? ? ? ? ? {

? ? ? ? ? ? ? ? ? ? Angle_turn = atan2(Yd,Xd);//目的地與小車的世界坐標系方位角

? ? ? ? ? ? ? ? }

? ? ? ? ? ? ? ? //逆時針轉(zhuǎn)向目標點

? ? ? ? ? ? ? ? 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;

? ? ? ? ? ? ? ? }

? ? ? ? ? ? ? ? //順時針轉(zhuǎn)向目標點

? ? ? ? ? ? ? ? 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;

? ? ? ? ? ? }

? ? ? ? ? ? //到達目的地后停止小車，任務結(jié)束

? ? ? ? ? ? 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; ?

? ? //調(diào)用回調(diào)函數(shù)

? ? ros::spin(); ?

? ? ?

? ? return 0; ?

} ?