本文共 12953 字，大约阅读时间需要 43 分钟。
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
package.xml
turtlebot_navigation 2.3.7 turtlebot_navigation OSRF BSD http://ros.org/wiki/turtlebot_navigation https://github.com/turtlebot/turtlebot_apps https://github.com/turtlebot/turtlebot_apps/issues Tully Foote catkin tf roscpp sensor_msgs tf roscpp sensor_msgs move_base map_server amcl gmapping turtlebot_bringup dwa_local_planner std_msgs kobuki_msgs geometry_msgs
2.send_goal.cpp
路径：turtlebot_navigation/src
#include #include #include #include #include #include #include #include #include #include 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 ac("move_base",true); //Define a marker publisher. marker_pub = node.advertise ("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 (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;}
检测到障碍物掉头并直行1m程序v
send_goalV1.cpp
#include #include #include #include #include #include /** * @ brief A simple bump-blink-controller *public yocs::Controller * A simple nodelet-based controller for Kobuki, which makes one of Kobuki's LEDs blink, when a bumper is pressed. */class BumpBlinkController {public: //BumpBlinkController(ros::NodeHandle& nh, std::string& name) : Controller(), nh_(nh), name_(name){}; BumpBlinkController(); void startMoving(); void turnAround();private: ros::NodeHandle nh_; std::string name_; ros::Subscriber enable_controller_subscriber_, disable_controller_subscriber_; ros::Subscriber bumper_event_subscriber_; ros::Publisher blink_publisher_; ros::Publisher commandPub; tf::TransformListener listener; tf::StampedTransform transform; bool keepMoving; void bumperEventCB(const kobuki_msgs::BumperEventConstPtr msg);};BumpBlinkController::BumpBlinkController(){ keepMoving=true; bumper_event_subscriber_ = nh_.subscribe("/mobile_base/events/bumper", 10, &BumpBlinkController::bumperEventCB, this); commandPub=nh_.advertise ("/cmd_vel_mux/input/navi",10,true); // choose between led1 and led2 blink_publisher_ = nh_.advertise< kobuki_msgs::Led >("commands/led1", 10);}void BumpBlinkController::startMoving(){ros::Rate rate(1);//Set the equivalent ROS rate variableROS_INFO("start moving");while(ros::ok()&&keepMoving){ ros::spinOnce(); //rate.sleep(); }}void BumpBlinkController::turnAround(){if(ros::ok()&&~keepMoving){ //ros::Rate loopRate(); ROS_INFO("turnning start..."); geometry_msgs::Twist speed; // תÍä¿ØÖÆÐźÅÔØÌå Twist message speed.linear.x = 0; speed.angular.z = 0; std::string odom_frame = "/odom"; std::string base_frame; //Set the forward linear speed to 0.2 meters per second float linear_speed = 0.2;//Set the travel distance to 1.0 meters float goal_distance = 0.5; //Set the rotation speed to 0.5 radians per second float angular_speed = 1.0; //Set the rotation angle to Pi radians (180 degrees) //Set the angular tolerance in degrees converted to radians double angular_tolerance = 2.5*M_PI/180; //œÇ¶Èת»»³É»¡¶È:deg*PI/180 try { listener.waitForTransform(odom_frame, "/base_footprint", ros::Time(), ros::Duration(2.0) ); base_frame = "/base_footprint"; ROS_INFO("base_frame = /base_footprint"); } catch (tf::TransformException & ex) { try { listener.waitForTransform(odom_frame, "/base_link", ros::Time(), ros::Duration(2.0) ); base_frame = "/base_link"; ROS_INFO("base_frame = /base_link"); } catch (tf::TransformException ex) { ROS_INFO("Cannot find transform between /odom and /base_link or /base_footprint"); commandPub.publish(geometry_msgs::Twist()); ros::shutdown(); } } //Stop the robot before the rotation commandPub.publish(geometry_msgs::Twist()); //ros::Duration(1).sleep(); // sleep for a second ROS_INFO("rotation...!"); //Now rotate left roughly 180 degrees speed.linear.x = 0; //Set the angular speed speed.angular.z = angular_speed; // ÉèÖÃœÇËٶȣ¬ÕýΪ×óת£¬žºÎªÓÒת //Track the last angle measured double last_angle = fabs(tf::getYaw(transform.getRotation())); //Track how far we have turned double turn_angle = 0; while( (fabs(turn_angle + angular_tolerance) < M_PI) && (ros::ok()) ) { //Publish the Twist message and sleep 1 cycle commandPub.publish(speed); //loopRate.sleep(); // Get the current rotation listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform); //C++: double rotation = fabs(tf::getYaw(transform.getRotation())); //Compute the amount of rotation since the last loop double delta_angle = fabs(rotation - last_angle); //Add to the running total turn_angle += delta_angle; last_angle = rotation; } //Stop the robot before the rotation //Set the angular speed speed.angular.z = 0; commandPub.publish(geometry_msgs::Twist()); ros::Duration(1).sleep(); // sleep for a secondv ROS_INFO("go straight...!"); //Set the angular speed speed.angular.z = 0; // ÉèÖÃœÇËٶȣ¬ÕýΪ×óת£¬žºÎªÓÒת speed.linear.x = linear_speed; // ÉèÖÃÏßËٶȣ¬ÕýΪǰœø£¬žºÎªºóÍË //Get the starting position values listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform); float x_start = transform.getOrigin().x(); float y_start = transform.getOrigin().y(); // Keep track of the distance traveled float distance = 0; while( (distance < goal_distance) && (ros::ok()) ) { //Publish the Twist message and sleep 1 cycle commandPub.publish(speed); //loopRate.sleep(); listener.lookupTransform(odom_frame, base_frame, ros::Time(0), transform); //Get the current position float x = transform.getOrigin().x(); float y = transform.getOrigin().y(); //Compute the Euclidean distance from the start distance = sqrt(pow((x - x_start), 2) + pow((y - y_start), 2)); } //Stop the robot before the rotation commandPub.publish(geometry_msgs::Twist()); //ros::Duration(1).sleep(); // sleep for a second keepMoving=true;}}void BumpBlinkController::bumperEventCB(const kobuki_msgs::BumperEventConstPtr msg){ if (msg->state == kobuki_msgs::BumperEvent::PRESSED) { ROS_INFO_STREAM("Bumper pressed. Turning around."); keepMoving=false; turnAround(); } else // kobuki_msgs::BumperEvent::RELEASED { ROS_INFO_STREAM("Bumper released.Go ahead"); keepMoving=true; startMoving(); }}int main(int argc,char **argv){ ros::init(argc,argv,"bumper_event"); BumpBlinkController bumper; bumper.startMoving(); //ros::spinOnce(); return 0;}
总结：
1.rqt_graph 查看消息与节点之间的关系
2.一个node,要么作为消息的发布者，要么作为消息的接受者，消息类型一致可订阅，消息类型一致，消息名字不一致的需要remap from to
转载地址：http://keqpi.baihongyu.com/