ROS通过程序设定起始点和目标点

C++

头文件

#ifndef GOAL_PUB_H
#define GOAL_PUB_H

#include <ros/ros.h>  
#include <move_base_msgs/MoveBaseAction.h>  
#include <actionlib/client/simple_action_client.h>  
#include "geometry_msgs/PoseWithCovarianceStamped.h"     
#include "std_msgs/String.h"

#define GOHOME "HOME"
#define GODRAMING "DRAMING"

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
 class goal_pub
 {
   
   
     public:
         typedef struct _POSE
         {
   
        
             double X;
             double Y;
             double Z;
             double th_x;
             double th_y;
             double th_z;
             double th_w;

         } POSE;
         MoveBaseClient* ac_;
         void setHome( ros::Publisher pub,POSE pose);
         void setGoal(POSE pose);

     private:
         tf::StampedTransform world_pose;
         tf::TransformListener listener_;
 };

#endif

源文件

#include "goal_pub.h"
using namespace std;

bool Callback_flag = false;
string msg_str = "";

goal_pub::POSE HOME = {
   
   0, 0, 0,  0, 0, 0, 0};
goal_pub::POSE pose1 = {
   
   1.484616458416 , 0.0, 0.0,  0.0, 0.0, 0, 0.0};
goal_pub::POSE pose2 = {
   
   2.15833854675  , 0.0, 0.0,  0.0, 0.0, 0, 0.0};

//设置起始点
void goal_pub::setHome( ros::Publisher pub,POSE pose)
{
   
   
    geometry_msgs::PoseWithCovarianceStamped msg_poseinit;
    msg_poseinit.header.frame_id = "map";
    msg_poseinit.header.stamp = ros::Time::now();
    msg_poseinit.pose.pose.position.x = pose.X;
    msg_poseinit.pose.pose.position.y = pose.Y;
    msg_poseinit.pose.pose.position.z = pose.Z;

    double