#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终端信息

填写以下坐标: <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>
/*

*

*

*/

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