1. 程式人生 > >turtlebot2自主導航move_base_grid除錯記錄

turtlebot2自主導航move_base_grid除錯記錄

1.terminal1,執行

roscore

2.turminal2 執行turtlebot節點

roslaunch turtlebot_bringup minimal.launch

3.terminal3啟動其他一系列節點

roslaunch rbx1_nav wfmovebase_grid.launch

4.執行導航程式

rosrun turtlebot_navigation send_goal

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
原始碼:
wfmovebase_grid.launch

<launch>
  <!-- Run the map server with a blank map -->
  <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/blank_map.yaml"/>
  
  <param name="use_sim_time" value="false" />

  <!-- Set the name of the map yaml file: can be overridden on the command line. -->
  <arg name="map" default="test_map.yaml" />

    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    <remap from="cmd_vel" to="/cmd_vel_mux/input/navi">
    <rosparam file="$(find rbx1_nav)/config/fake/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find rbx1_nav)/config/fake/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find rbx1_nav)/config/fake/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find rbx1_nav)/config/fake/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find rbx1_nav)/config/fake/base_local_planner_params.yaml" command="load" />
  </node>

  <!-- Run a static transform between /odom and /map -->
  <node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />

</launch>

2.send_goal.cpp
路徑:turtlebot_navigation/src

#include <ros/ros.h>
#include <signal.h>
#include <geometry_msgs/Twist.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 <visualization_msgs/Marker.h>
#include <cmath>

ros::Publisher cmdVelPub;
ros::Publisher marker_pub;

void shutdown(int sig)
{
  cmdVelPub.publish(geometry_msgs::Twist());
  ros::Duration(1).sleep(); // sleep for  a second
  ROS_INFO("nav_square.cpp ended!");
  ros::shutdown();
}

void init_markers(visualization_msgs::Marker *marker)
{
  marker->ns       = "waypoints";
  marker->id       = 0;
  marker->type     = visualization_msgs::Marker::CUBE_LIST;
  marker->action   = visualization_msgs::Marker::ADD;
  marker->lifetime = ros::Duration();//0 is forever
  marker->scale.x  = 0.2;
  marker->scale.y  = 0.2;
  marker->color.r  = 1.0;
  marker->color.g  = 0.7;
  marker->color.b  = 1.0;
  marker->color.a  = 1.0;

  marker->header.frame_id = "odom";
  marker->header.stamp = ros::Time::now();

}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "nav_move_base");
  std::string topic = "/cmd_vel";
  ros::NodeHandle node;
  //Subscribe to the move_base action server
  actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base",true);

  //Define a marker publisher.
  marker_pub = node.advertise<visualization_msgs::Marker>("waypoint_markers", 10);

  //for init_markers function
  visualization_msgs::Marker  line_list;

  signal(SIGINT, shutdown);
  ROS_INFO("move_base_square.cpp start...");

  //How big is the square we want the robot to navigate?
  double square_size = 0.5;

  //Create a list to hold the target quaternions (orientations)
  geometry_msgs::Quaternion quaternions[13];

  //convert the angles to quaternions
  double angle = M_PI/2;
 // double angle = 0;
  int angle_count = 0;
/*  for(angle_count = 0; angle_count < 4;angle_count++ )
  {
   quaternions[angle_count] = tf::createQuaternionMsgFromRollPitchYaw(0, 0, angle);
   angle = angle + M_PI/2;
//	angle = 0;
  }*/

  quaternions[0] = tf::createQuaternionMsgFromRollPitchYaw(0, 0, M_PI / 2);
  quaternions[1] = tf::createQuaternionMsgFromRollPitchYaw(0, 0, M_PI );
  quaternions[2] = tf::createQuaternionMsgFromRollPitchYaw(0, 0, M_PI);
  quaternions[3] = tf::createQuaternionMsgFromRollPitchYaw(0, 0, M_PI / 2);
  quaternions[4] = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0);
  quaternions[5] = tf::createQuaternionMsgFromRollPitchYaw(0, 0, M_PI / 2);
  quaternions[6] = tf::createQuaternionMsgFromRollPitchYaw(0, 0, M_PI );
  quaternions[7] = tf::createQuaternionMsgFromRollPitchYaw(0, 0, M_PI / 2);
  quaternions[8] = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0);
  quaternions[9] = tf::createQuaternionMsgFromRollPitchYaw(0, 0, M_PI / 2);
  quaternions[10] = tf::createQuaternionMsgFromRollPitchYaw(0, 0, M_PI);
  quaternions[11] = tf::createQuaternionMsgFromRollPitchYaw(0, 0, M_PI / 2);
  quaternions[12] = tf::createQuaternionMsgFromRollPitchYaw(0, 0, 0);


  //a pose consisting of a position and orientation in the map frame.
  geometry_msgs::Point point;
  geometry_msgs::Pose pose_list[13];
  //point.x = square_size;
  point.x = 5.0;
  point.y = 0.0;
  point.z = 0.0;
  pose_list[0].position = point;
  pose_list[0].orientation = quaternions[0];
  //point.x = square_size;
  //point.x = 6.0;
  //point.y = square_size;
  //point.y = 1.5;
  //point.z = 0.0;
  //pose_list[1].position = point;
  //pose_list[1].orientation = quaternions[1];

  point.x = 5.0;
  //point.y = square_size;
  point.y = 0.5;
  point.z = 0.0;
  pose_list[1].position = point;
  pose_list[1].orientation = quaternions[1];

  point.x = 0.0;
  point.y = 0.5;
  point.z = 0.0;
  pose_list[2].position = point;
  pose_list[2].orientation = quaternions[2];

  point.x = 0.0;
  point.y = 1;
  point.z = 0.0;
  pose_list[3].position = point;
  pose_list[3].orientation = quaternions[3];

  point.x = 0.0;
  point.y = 1;
  point.z = 0.0;
  pose_list[4].position = point;
  pose_list[4].orientation = quaternions[4];

  point.x = 5.0;
  point.y = 1;
  point.z = 0.0;
  pose_list[5].position = point;
  pose_list[5].orientation = quaternions[5];

  point.x = 5.0;
  point.y = 1.5;
  point.z = 0.0;
  pose_list[6].position = point;
  pose_list[6].orientation = quaternions[6];

  point.x = 0.0;
  point.y = 1.5;
  point.z = 0.0;
  pose_list[7].position = point;
  pose_list[7].orientation = quaternions[7];

  point.x = 0.0;
  point.y = 2.0;
  point.z = 0.0;
  pose_list[8].position = point;
  pose_list[8].orientation = quaternions[8];


  point.x = 5.0;
  point.y = 2.0;
  point.z = 0.0;
  pose_list[9].position = point;
  pose_list[9].orientation = quaternions[9];


  point.x = 5.0;
  point.y = 2.5;
  point.z = 0.0;
  pose_list[10].position = point;
  pose_list[10].orientation = quaternions[10];


  point.x = 0.0;
  point.y = 2.5;
  point.z = 0.0;
  pose_list[11].position = point;
  pose_list[11].orientation = quaternions[11];


  point.x = 0.0;
  point.y = 3.0;
  point.z = 0.0;
  pose_list[12].position = point;
  pose_list[12].orientation = quaternions[12];






  //Initialize the visualization markers for RViz
  init_markers(&line_list);

  //Set a visualization marker at each waypoint
  for(int i = 0; i < 13; i++)
  {
    line_list.points.push_back(pose_list[i].position);
  }

  //Publisher to manually control the robot (e.g. to stop it, queue_size=5)
  cmdVelPub = node.advertise<geometry_msgs::Twist>(topic, 5);



  ROS_INFO("Waiting for move_base action server...");
  //Wait 60 seconds for the action server to become available
  if(!ac.waitForServer(ros::Duration(180)))
  {
    ROS_INFO("Can't connected to move base server");
    return 1;
  }
  ROS_INFO("Connected to move base server");
  ROS_INFO("Starting navigation test");



  //Cycle through the four waypoints
  while(ros::ok())
  {

  //Initialize a counter to track waypoints
  int count = 0;

  while( (count < 13) )
  {
     //Update the marker display
     marker_pub.publish(line_list);

     //Intialize the waypoint goal
     move_base_msgs::MoveBaseGoal goal;

     //Use the map frame to define goal poses
     goal.target_pose.header.frame_id = "map";

     //Set the time stamp to "now"
     goal.target_pose.header.stamp = ros::Time::now();
     
     //Set the goal pose to the i-th waypoint
     goal.target_pose.pose = pose_list[count];

     //Start the robot moving toward the goal
     //Send the goal pose to the MoveBaseAction server
     ac.sendGoal(goal);

    //Allow 3 minute to get there
    bool finished_within_time = ac.waitForResult(ros::Duration(30));

    //If we dont get there in time, abort the goal
    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");
        }
    }
     count += 1;
  }
  }

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

總結:
1.rqt_graph 檢視訊息與節點之間的關係
2.一個node,要麼作為訊息的釋出者,要麼作為訊息的接受者,訊息型別一致可訂閱,訊息型別一致,訊息名字不一致的需要remap from to