## page was renamed from arm_planning_environment/Tutorials/Tutorial A ## page was renamed from arm_planning_environment/Tutorials/Tutorial 2 ## page was renamed from arm_planning_environment/Tutorials/Tutorial 1 ## For instruction on writing tutorials ## http://www.ros.org/wiki/WritingTutorials #################################### ##FILL ME IN #################################### ## for a custom note with links: ## note = This tutorial only works with Diamondback. For a roughly equivalent tutorial for Electric please see [[http://www.ros.org/wiki/arm_navigation/Tutorials/Planning%20Scene/Checking%20State%20Validity|Checking state validity]] ## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links ## note.0= [[motion_planning_environment/Tutorials/Making%20collision%20maps%20from%20self-filtered%20tilting%20laser%20data | Making collision maps]] ## descriptive title for the tutorial ## title = Checking collisions for a given robot state ## multi-line description to be displayed in search ## description = This tutorial will show you how to use the environment server with laser collision map data to check whether a given robot state is collision free, within the joint limits and satisfies joint or cartesian constraints. ## the next tutorial description (optional) ## next = ## links to next tutorial (optional) ## next.0.link=[[motion_planning_environment/Tutorials/Adding known objects to the collision environment| Adding known objects to the collision environment]] ## next.1.link= ## what level user is this tutorial for ## level= AdvancedCategory ## keywords = collision limit constraint #################################### <> <> In this tutorial we will show how a node called the environment server can consume collision maps and check whether particular robot states represent collisions, violations of joint limits, or violate Cartesian constraints. We will first create a short program that actually makes the call to the environment server to check a robot state for collision and then show the feedback that comes from a running environment server. == ROS Setup == In this tutorial, you will learn how to use the environment_server node to check collisions, joint limits and constraints for a given robot state. If you haven't already done so, the first thing we'll need to do is create a package for all the motion planning environment tutorials that we will work with. To do this we'll use the handy roscreate-pkg command where we want to create the package directory with a dependency on the pr2_arm_navigation_tutorials package as shown below: {{{ roscreate-pkg arm_navigation_tutorials pr2_arm_navigation_tutorials }}} After this is done we'll need to roscd to the package we created, since we'll be using it as our workspace {{{ roscd arm_navigation_tutorials }}} Also, make sure to set an environment variable called ROBOT to '''sim'''. E.g., in a bash shell, {{{ export ROBOT=sim }}} (Note: you need to do this only if you are running this tutorial on a simulator). == Creating the Node == Now that we have our package, we need to write the code that will call the query service. Fire up a text editor and paste the following into a file called src/get_state_validity.cpp. Don't worry if there are things you don't understand, we'll walk through the details of this file line-by-line shortly. {{{ #!cplusplus block=tree_block #include #include int main(int argc, char **argv){ ros::init (argc, argv, "get_state_validity_test"); ros::NodeHandle rh; ros::service::waitForService("environment_server/get_state_validity"); ros::ServiceClient check_state_validity_client_ = rh.serviceClient("environment_server/get_state_validity"); planning_environment_msgs::GetStateValidity::Request req; planning_environment_msgs::GetStateValidity::Response res; req.robot_state.joint_state.name.push_back("r_shoulder_pan_joint"); req.robot_state.joint_state.name.push_back("r_shoulder_lift_joint"); req.robot_state.joint_state.name.push_back("r_upper_arm_roll_joint"); req.robot_state.joint_state.name.push_back("r_elbow_flex_joint"); req.robot_state.joint_state.name.push_back("r_forearm_roll_joint"); req.robot_state.joint_state.name.push_back("r_wrist_flex_joint"); req.robot_state.joint_state.name.push_back("r_wrist_roll_joint"); req.robot_state.joint_state.position.resize(7,0.0); //these set whatever non-zero joint angle values are desired req.robot_state.joint_state.position[0] = 0.0; req.robot_state.joint_state.header.stamp = ros::Time::now(); req.check_collisions = true; if(check_state_validity_client_.call(req,res)) { if(res.error_code.val == res.error_code.SUCCESS) ROS_INFO("Requested state is not in collision"); else ROS_INFO("Requested state is in collision. Error code: %d",res.error_code.val); } else { ROS_ERROR("Service call to check state validity failed %s",check_state_validity_client_.getService().c_str()); return false; } ros::shutdown(); } }}} == Creating the service request == The service request to the !GetStateValidity service requires the following information: * A set of joint positions that you would like to check * The types of checks to be carried out === Filling up the joint state === <> We first fill up the joint state with desired positions for the right arm. These are example positions, you can change and play around with them if you like. === Types of checks === The types of checks that can be performed on the input robot state are specified within the service request itself as individual flags. <> There are currently 4 types of checks that you can ask to be carried out. Each type of check can be activated by setting the corresponding flag. * collision checks: {{{request.check_collisions = true;}}} * joint limits checks: {{{request.check_joint_limits = true;}}} * goal constraints test: {{{request.check_goal_constraints = true;}}} * path constraints test: {{{request.check_path_constraints = true;}}} == Building the node == Now that we have a package and a source file, we'll want to build and then try things out. The first step will be to add our `src/get_state_validity.cpp` file to our `CMakeLists.txt` file to get it to build. Open up `CMakeLists.txt` in your editor of choice and add the following line to the bottom of the file. {{{ rosbuild_add_executable(get_state_validity src/get_state_validity.cpp) }}} Once this is done, we can build our executable by typing make. {{{ rosmake }}} == Launching the PR2 in simulation == We'll be using the PR2 to illustrate all the concepts in this tutorial, and thus the first step is to launch the PR2 in simulation. To launch the PR2 in simulation assuming that the pr2_arm_navigation_tutorials in your ROS_PACKAGE_PATH: {{{ roslaunch pr2_arm_navigation_tutorials pr2_floorobj_world.launch }}} You should get something that looks like this configuration: {{attachment:gazebo_floorobj.jpg| gazebo with pole | width=800}} == Launching rviz with preconfigured topics == In order to view the data topics we use in this tutorial run the following command to launch rviz with a preconfigured set of topics: {{{ roslaunch pr2_arm_navigation_tutorials rviz_collision_tutorial_2.launch }}} == Launch the laser perception pipeline == Next we need to launch the tilting laser to filtered collision map pipeline we created in the [[motion_planning_environment/Tutorials/Making collision maps from self-filtered tilting laser data|previous tutorial]]: {{{ roslaunch pr2_arm_navigation_tutorials laser-perception.launch }}} The laser should begin nodding and if you enable the sixth topic in the rviz configuration (collision map) you should see a number of green squares representing the collision map: {{attachment:rviz_collision_map.jpg| rviz showing collision map | width=800}} === Setup the environment server === Now, launch the environment server. {{{ roslaunch pr2_arm_navigation_actions environment_server.launch }}} You should see the following output: {{{ process[environment_server-1]: started with pid [16573] [ INFO] 1417.882999973: Configuring action for 'right_arm' [ INFO] 1417.882999973: Waiting for robot state ... [ INFO] 1417.882999973: Waiting for joint state ... [ INFO] 1417.882999973: Pose ignored [ WARN] 1417.882999973: Message from [/collision_map_self_occ_node] has a non-fully-qualified frame_id [base_link]. Resolved locally to [/base_link]. This is will likely not work in multi-robot systems. This message will only print once. [ INFO] 1417.883999973: Ignoring joint 'r_gripper_joint' [ INFO] 1417.883999973: Ignoring joint 'l_gripper_joint' [ INFO] 1417.933999973: Robot state received! [ INFO] 1417.934999973: Waiting for map ... [ INFO] 1419.052999973: Map received! [ INFO] 1419.070999973: Found collisions will be displayed as visualization markers [ INFO] 1419.102999973: Environment monitor started }}} === Run the code in a collision free configuration === Next enable the 9th rviz topic marked ''State Validity''. This will cause another ghosted robot to appear in rviz floating near the existing robot. This ghosted robot will display the position we are checking for validity. Then run the executable we just created {{{ ./bin/get_state_validity }}} If it succeeds, you should see the following output {{{ [ INFO] 0.000000000: Requested state is not in collision }}} In rviz you should see something like the following: {{attachment:rviz_state_validity_1.jpg| rviz showing no collisions | width=800}} The ghosted robot shows the position with all joint links at 0.0 given that the rest of the robot is in its current position; this state is not in collision with anything and is within all joint limits, and the environment server confirms that. To see what happens if we ask for a state in collision, change the following line in get_state_validity.cpp: <> to {{{ req.robot_state.joint_state.position[0] = -0.55; }}} This tells the environment server to check the state where the first joint ("r_shoulder_pan_link") has a value of -0.55, with all other values left at 0.0. Before re-running get_state_validity make sure that the eighth rviz entry (Collision markers), is enabled. Recompile and run get_state_validity again. If it succeeds, you should see the following output {{{ [ INFO] 0.000000000: Requested state is in collision. Error code: -23 }}} The error code corresponds to the error codes in the [[http://www.ros.org/doc/api/motion_planning_msgs/html/msg/ArmNavigationErrorCodes.html|ArmNavigationErrorCodes]] message in the motion_planning_msgs package. In rviz you see something like this: {{attachment:rviz_state_validity_2.jpg| rviz showing collisions with pole | width=800}} The environment server has reported that the given state will be in collision. What it doesn't say is what collisions were present. In order to determine that we must consult rviz. When the environment server is asked to check a state that collides with something in the environment it broadcasts markers. If you had Collision Markers enabled when you ran your program you should have a number of small yellow spheres in the middle of the collision map hits where the pole is. It may help to disable the collision map to see them. These points represent some (but potentially not all) of the collisions that resulted from the state validity check. The spheres are centered when the collisions were detected. To see what collided with what at those points, choose ''Select'' from the rviz toolbar and select one or more points. In the rviz selection window you should see a number of markers with named "r_forearm_link+points". This indicates that at the requested arm position the right forearm link would hit the points from the collision map. Colliding with objects in the environment is only one kind of collision the environment server can detect. To illustrate another kind of collision, again edit get_state_validity.cpp, editing the line to read: {{{ req.robot_state.joint_state.position[0] = 0.4; }}} Recompile and run get_state_validity again. You should again see: {{{ [ INFO] 0.000000000: Requested state is in collision. Error code: -23 }}} This time you should get yellow collision spheres by the robot's left arm, as shown in the this screenshot: {{attachment:rviz_state_validity_3.jpg| rviz showing collisions with pole | width=800}} Selecting a collision should show one left arm link + one right arm link. This is a self-collision, where the robot will hit itself with the arms in the given position. These kind of collisions should generally also be avoided, and the environment server gives the ability to check for them. ## AUTOGENERATED DO NOT DELETE ## TutorialCategory ## FILL IN THE STACK TUTORIAL CATEGORY HERE