版权声明：本文为博主原创文章,转载请注明出处 https://blog.csdn.net/sophies671207/article/details/85387719
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