We have seen how to send a goal position to a robot for moving it from point A to B, using the RViz 2D Nav Goal button. Now we will see how to command the robot using actionlib client and ROS C++ APIs. Following is a sample package and node for communicating with Navigation stack move_base node.
The move_base node is SimpleActionServer. We can send and cancel the goals to the robot if the task takes a lot of time to complete.
The following code is SimpleActionClient for the move_base node, which can send the x, y, and theta from the command line arguments. The following code is in the chefbot_bringup/src folder with the name of send_robot_goal.cpp:
#include <ros/ros.h> #include <move_base_msgs/MoveBaseAction.h> #include <actionlib/client/simple_action_client.h> #include <tf/transform_broadcaster.h> #include <sstream> #include <iostream> //Declaring a new SimpleActionClient with action of move_base_msgs::MoveBaseAction typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient; int main(int argc, char** argv){ ros::init(argc, argv, "navigation_goals"); //Initiating move_base client MoveBaseClient ac("move_base", true); //Waiting for server to start while(!ac.waitForServer(ros::Duration(5.0))){ ROS_INFO("Waiting for the move_base action server"); } //Declaring move base goal move_base_msgs::MoveBaseGoal goal; //Setting target frame id and time in the goal action goal.target_pose.header.frame_id = "map"; goal.target_pose.header.stamp = ros::Time::now(); //Retrieving pose from command line other vice execute a default value try{ goal.target_pose.pose.position.x = atof(argv[1]); goal.target_pose.pose.position.y = atof(argv[2]); goal.target_pose.pose.orientation.w = atof(argv[3]); } catch(int e){ goal.target_pose.pose.position.x = 1.0; goal.target_pose.pose.position.y = 1.0; goal.target_pose.pose.orientation.w = 1.0; } ROS_INFO("Sending move base goal"); //Sending goal ac.sendGoal(goal); ac.waitForResult(); if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) ROS_INFO("Robot has arrived to the goal position"); else{ ROS_INFO("The base failed for some reason"); } return 0; }
The following lines are added to CMakeLists.txt for building this node:
add_executable(send_goal src/send_robot_goal.cpp) target_link_libraries(send_goal ${catkin_LIBRARIES} )
Build the package using catkin_make and test the working of the client using the following set of commands using Gazebo.
Start Gazebo simulation in a room:
$ roslaunch chefbot_gazebo chefbot_room_world.launch
Start the amcl node with the generated map:
$ roslaunch chefbot_gazebo amcl_demo.launch map_file:=final_room.yaml
Start RViz for navigation:
$ roslaunch chefbot_bringup view_navigation.launch
Run the send goal node for sending the move base goal:
$ rosrun chefbot_bringup send_goal 1 0 1
We will see the red arrow appear when this node runs, which shows that the pose is set on RViz.
After completing the operation, we will see the following messages in the send
goal terminal:
We will get the desired pose of the robot in the map by using the RViz 2D Nav goal button. Simply echoing the topic /move_base/goal will print the pose that we commanded through RViz. We can use these values as command line arguments in the send_goal node.