ros机器人导航设置原点，目标点

 

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <string.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <cmath>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "nav_move_base");
  ros::NodeHandle node;
  //订阅move_base操作服务器
  actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base",true);

  ros::Publisher odom_pub = node.advertise<nav_msgs::Odometry>("odom", 50);

  ros::Time current_time, last_time;
  current_time = ros::Time::now();

for(double i=-2;i<3;i++)
{
  for(double j=-2;j<3;j++)
 {
  //设置我们要机器人走的几个点。
    geometry_msgs::Point point;
    geometry_msgs::Quaternion quaternion;
    geometry_msgs::Pose pose_list[0];
    
    point.x = i;
    point.y = j;
    point.z = 0.000;
    quaternion.x = 0.000;
    quaternion.y = 0.000;
    quaternion.z = 0.012;
    quaternion.w = 0.999;
    pose_list[0].position = point;
    pose_list[0].orientation = quaternion;
        

  ROS_INFO("Waiting for move_base action server...");
  //等待60秒以使操作服务器可用
  if(!ac.waitForServer(ros::Duration(30)))
  {
    ROS_INFO("Can't connected to move base server");
    return 1;
  }
  ROS_INFO("Connected to move base server");
  ROS_INFO("Starting navigation test");

  //循环通过四个航点

     //初始化航点目标
     move_base_msgs::MoveBaseGoal goal;

     //使用地图框定义目标姿势
     goal.target_pose.header.frame_id = "map";

     //将时间戳设置为“now”
     goal.target_pose.header.stamp = ros::Time::now();

     //将目标姿势设置为第i个航点
     goal.target_pose.pose = pose_list[0];

     //让机器人向目标移动
     //将目标姿势发送到MoveBaseAction服务器
     ac.sendGoal(goal);

    //等1分钟到达那里
    bool finished_within_time = ac.waitForResult(ros::Duration(8));

    //如果我们没有及时赶到那里，就会中止目标
    if(!finished_within_time)
    {
        ac.cancelGoal();
        ROS_INFO("Timed out achieving goal");
    }
    else
    {
        //We made it!
        if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        {
            ROS_INFO("Goal succeeded!");
        }
        else
        {
            ROS_INFO("The base failed for some reason");
        }
    }
 }
}

 //next, we'll publish the odometry message over ROS接下来，我们将在ROS上发布odometry消息
    nav_msgs::Odometry odom;
    odom.header.frame_id = "odom";

    //set the position
    odom.pose.pose.position.x = 0.0;
    odom.pose.pose.position.y = 0.0;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation.x = 0.000;
    odom.pose.pose.orientation.y = 0.000;
    odom.pose.pose.orientation.z = 0.000;
    odom.pose.pose.orientation.w = 1.000;

    //set the velocity
    odom.child_frame_id = "base_link";
    odom.twist.twist.linear.x = 0.0;
    odom.twist.twist.linear.y = 0.0;
    odom.twist.twist.angular.z = 0.0;

    //publish the message
    odom_pub.publish(odom);

  ros::spin();
  ROS_INFO("move_base_square.cpp end...");
  return 0;
}

之前利用movebase导航定位都是通过rviz用鼠标指来指去，实验时非常方便，但实际应用总不能也人工指来指去吧，这怎么体现智能呢

启动导航后，用以前使用的rviz设设置目标点来获取map坐标系下的位置坐标

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

 

查看rviz终端信息

1 2 3 4 5 6 7 8 9 10

填写以下坐标： <br>    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;<br><br>

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144

/* * * */ #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; 　　<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 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;  }