turtlebot2自主導航move_base_grid除錯記錄
阿新 • • 發佈:2018-12-30
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