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 roll,pitch,yaw;
    roll=pose.th_x;
    pitch=pose.th_y;
    yaw=pose.th_z;

    //欧拉角转四元数
    geometry_msgs::Quaternion q;
    q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);  

    msg_poseinit.pose.pose.orientation.x = q.x;
    msg_poseinit.pose.pose.orientation.y = q.y;
    msg_poseinit.pose.pose.orientation.z = q.z;
    msg_poseinit.pose.pose.orientation.w = q.w;
// 　　<br>　　/×因为ros话题原理本身的问题，Setting pose 需要按照以下发送×/<br>

    pub.publish(msg_poseinit);
    ros::Duration(1.0).sleep();
    pub.publish(msg_poseinit);
    ros::Duration(1.0).sleep();
    pub.publish(msg_poseinit);
    ros::Duration(1.0).sleep();
	
}

//设置目标点
void goal_pub::setGoal(POSE pose)
{
     //tell the action client that we want to spin a thread by default 
    MoveBaseClient ac("move_base", true); 
       
    //wait for the action server to come up 
    while(!ac.waitForServer(ros::Duration(5.0)))
    { 
        ROS_WARN("Waiting for the move_base action server to come up"); 
    } 
    move_base_msgs::MoveBaseGoal goal; 
  
    double roll,pitch,yaw;
    roll=pose.th_x;
    pitch=pose.th_y;
    yaw=pose.th_z;

    //欧拉角转四元数
    geometry_msgs::Quaternion q;
    q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);  
    
    //we'll send a goal to the robot to move 1 meter forward 
    goal.target_pose.header.frame_id = "map"; 
    goal.target_pose.header.stamp = ros::Time::now(); 
 
    goal.target_pose.pose.position.x = pose.X;
    goal.target_pose.pose.position.y = pose.Y; 
    goal.target_pose.pose.position.z = pose.Z; 
    goal.target_pose.pose.orientation.x = q.x;
    goal.target_pose.pose.orientation.y = q.y;
    goal.target_pose.pose.orientation.z = q.z;
    goal.target_pose.pose.orientation.w = q.w;  
     
    ROS_INFO("Sending goal"); 
    ac.sendGoal(goal); 
    ac.waitForResult(); 
    if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) 
      ROS_INFO("it is successful"); 
     else 
       ROS_ERROR("The base failed  move to goal!!!");  
}

void poseCallback(const std_msgs::String::ConstPtr &msg)
{
     ROS_INFO_STREAM("Topic is Subscriber ");
     std::cout<<"get topic text: " << msg->data << std::endl;
     Callback_flag = true;
     msg_str = msg->data;
}   

int main (int argc, char **argv)
{
ros::init(argc, argv, "base_pose_control"); 
ros::NodeHandle nh;
//发布位姿估计消息，设定起始点
ros::Publisher pub_initialpose = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 10);
ros::Subscriber sub = nh.subscribe("/base/pose_topic",10,poseCallback); 
goal_pub goal;

ros::Rate rate_loop(10);
goal.setHome(pub_initialpose,HOME);

while(ros::ok())
  {
      if(Callback_flag == true)
      {
        Callback_flag = false;
        if(msg_str == GOHOME)
        {
             msg_str = "";
             goal.setGoal(pose1);
        }
        else  if(msg_str == GODRAMING)
        {
             msg_str = "";
             goal.setGoal(pose2);
        }
        else
        {
        }
      }
      ros::spinOnce();
     rate_loop.sleep();
  }
  return 0;
}

Python

#!/usr/bin/env python
# -*- coding: UTF-8 -*-
import rospy
import actionlib
from actionlib_msgs.msg import *
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseWithCovarianceStamped
from tf_conversions import transformations
from math import pi
import time

class Goal:
    arrived=0
    def __init__(self):
        rospy.loginfo('wait move_base server ......')
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)#wait move_base server
        self.move_base.wait_for_server(rospy.Duration(60))
        rospy.loginfo('move_base server actived !')
        self.set_pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=5)
        self.pub_cancel = rospy.Publisher("move_base/cancel",GoalID,queue_size=5)

    def set_pose(self, p):
        if self.move_base is None:
            return False
        x, y, th = p
        pose = PoseWithCovarianceStamped()
        pose.header.stamp = rospy.Time.now()
        pose.header.frame_id = 'map'
        pose.pose.pose.position.x = x
        pose.pose.pose.position.y = y
        q = transformations.quaternion_from_euler(0.0, 0.0, th/180.0*pi)
        pose.pose.pose.orientation.x = q[0]
        pose.pose.pose.orientation.y = q[1]
        pose.pose.pose.orientation.z = q[2]
        pose.pose.pose.orientation.w = q[3]
        self.set_pose_pub.publish(pose)
        print('set home success')
        return True

    def goto(self, p):
        goal = MoveBaseGoal()
        goal.target_pose.header.frame_id = 'map'
        goal.target_pose.header.stamp = rospy.Time.now()
        goal.target_pose.pose.position.x = p[0]
        goal.target_pose.pose.position.y = p[1]
        q = transformations.quaternion_from_euler(0.0, 0.0, p[2]/180.0*pi)
        goal.target_pose.pose.orientation.x = q[0]
        goal.target_pose.pose.orientation.y = q[1]
        goal.target_pose.pose.orientation.z = q[2]
        goal.target_pose.pose.orientation.w = q[3]
        self.move_base.send_goal(goal, self._done_cb, self._active_cb, self._feedback_cb)
        while not rospy.is_shutdown():
            # print self.move_base.get_state()
            if self.arrived == 1:
                self.arrived=0
                break
            time.sleep(0.5)
        return True
    
    def cancel_goal(self):
        data = GoalID()
        for i in range(0,2):
            self.pub_cancel.publish(data)
            time.sleep(0.5)
            
    def cancel(self):
        self.move_base.cancel_all_goals()
        return True

    def _done_cb(self, status, result):
        # print (result)
        if status==2: 
            self.arrived=1
            rospy.loginfo('target canceled !!')
        if status==3: 
            self.arrived=1
            rospy.loginfo('target arrived !')
            
    def _active_cb(self):
        rospy.loginfo("target received ")

    def _feedback_cb(self, feedback):
        rospy.loginfo("[Navi] navigation feedback\r\n%s"%feedback)