ros機器人導航設置原點，目標點

之前利用movebase導航定位都是通過rviz用鼠標指來指去，實驗時非常方便，但實際應用總不能也人工指來指去吧，這怎么體現智能呢

啟動導航后，用以前使用的rviz設設置目標點來獲取map坐標系下的位置坐標

使用 2d Nav Goal 指你想要的家坐標

查看rviz終端信息

填寫以下坐標： 
    geometry_msgs::PoseWithCovarianceStamped msg_poseinit;
    msg_poseinit.header.frame_id = "map";
    msg_poseinit.header.stamp = ros::Time::now(); 
    msg_poseinit.pose.pose.position.x = -0.644479990005;
    msg_poseinit.pose.pose.position.y = 2.2030518055;
    msg_poseinit.pose.pose.position.z = 0;
    msg_poseinit.pose.pose.orientation.x = 0.0;
    msg_poseinit.pose.pose.orientation.y = 0.0;
    msg_poseinit.pose.pose.orientation.z = -0.746261929753;
    msg_poseinit.pose.pose.orientation.w = 0.665652410949;

/*
*
*
*/
#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"

using namespace std;

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;  

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

bool Callback_flag = false;
string msg_str = "";

/*******************************默認amcl初始點******************************************/
typedef struct _POSE
{
  double X;
  double Y;
  double Z;
  double or_x;
  double or_y;
  double or_z;
  double or_w;
} POSE;

POSE pose2 = {-8.15833854675, 3.15512728691, 0.0,  0.0, 0.0, -0.740479961141, 0.672078438241};
POSE pose1 = {-0.484616458416, 2.13149046898, 0.0,  0.0, 0.0, -0.749884700297, 0.661568542375};
POSE pose3 = {0.0, 0.0, 0.0,  0.0, 0.0, 0.0, 0.0};
POSE pose4 = {0.0, 0.0, 0.0,  0.0, 0.0, 0.0, 0.0};

void setHome( ros::Publisher pub)
{
    geometry_msgs::PoseWithCovarianceStamped msg_poseinit;
    msg_poseinit.header.frame_id = "map";
    msg_poseinit.header.stamp = ros::Time::now(); 
    msg_poseinit.pose.pose.position.x = -0.644479990005;
    msg_poseinit.pose.pose.position.y = 2.2030518055;
    msg_poseinit.pose.pose.position.z = 0;
    msg_poseinit.pose.pose.orientation.x = 0.0;
    msg_poseinit.pose.pose.orientation.y = 0.0;
    msg_poseinit.pose.pose.orientation.z = -0.746261929753;
    msg_poseinit.pose.pose.orientation.w = 0.665652410949;
　　
　　/×因為ros話題原理本身的問題，Setting pose 需要按照以下發送×/

    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 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;  
      
    //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 = pose.or_x;
    goal.target_pose.pose.orientation.y = pose.or_y;
    goal.target_pose.pose.orientation.z = pose.or_z;
    goal.target_pose.pose.orientation.w = pose.or_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);  

  //ros::Rate rate_loop(10);

  setHome(pub_initialpose);
  
 // setGoal(pose1);
  while(ros::ok())
  {
      if(Callback_flag == true)
      {
        Callback_flag = false;

        if(msg_str == GOHOME)
        {
            msg_str = "";
            setGoal(pose1);
        }
        else  if(msg_str == GODRAMING)
        {
            msg_str = "";
            setGoal(pose2);
        }
        else
        {

        }
      }

      ros::spinOnce();
     // rate_loop.sleep();
  }
  

  return 0;  
}