get_state_validity.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <arm_navigation_msgs/GetPlanningScene.h>
00003 #include <planning_environment/models/collision_models.h>
00004 
00005 static const std::string GET_PLANNING_SCENE_NAME = "/environment_server/get_planning_scene";
00006 
00007 int main(int argc, char **argv){
00008   ros::init (argc, argv, "get_state_validity_test");
00009   ros::NodeHandle rh;
00010 
00011   ros::Publisher vis_marker_publisher_;
00012   ros::Publisher vis_marker_array_publisher_;
00013 
00014   vis_marker_publisher_ = rh.advertise<visualization_msgs::Marker>("state_validity_markers", 128);
00015   vis_marker_array_publisher_ = rh.advertise<visualization_msgs::MarkerArray>("state_validity_markers_array", 128);
00016 
00017   ros::service::waitForService(GET_PLANNING_SCENE_NAME);
00018   ros::ServiceClient get_planning_scene_client = 
00019     rh.serviceClient<arm_navigation_msgs::GetPlanningScene>(GET_PLANNING_SCENE_NAME);
00020 
00021   arm_navigation_msgs::GetPlanningScene::Request planning_scene_req;
00022   arm_navigation_msgs::GetPlanningScene::Response planning_scene_res;
00023 
00024   if(!get_planning_scene_client.call(planning_scene_req, planning_scene_res)) {
00025     ROS_WARN("Can't get planning scene");
00026     return -1;
00027   }
00028 
00029   planning_environment::CollisionModels collision_models("robot_description");
00030   planning_models::KinematicState* state = 
00031     collision_models.setPlanningScene(planning_scene_res.planning_scene);
00032 
00033   std::vector<std::string> arm_names = 
00034     collision_models.getKinematicModel()->getModelGroup("right_arm")->getUpdatedLinkModelNames();
00035   std::vector<std::string> joint_names = 
00036     collision_models.getKinematicModel()->getModelGroup("right_arm")->getJointModelNames();
00037 
00038   if(argc > 1) {
00039     std::stringstream s(argv[1]);
00040     double val;
00041     s >> val;
00042     std::map<std::string, double> nvalues;
00043     nvalues["r_shoulder_pan_joint"] = val;
00044     
00045     state->setKinematicState(nvalues);
00046   }
00047   
00048   std_msgs::ColorRGBA good_color, collision_color, joint_limits_color;
00049   good_color.a = collision_color.a = joint_limits_color.a = .8;
00050 
00051   good_color.g = 1.0;
00052   collision_color.r = 1.0;
00053   joint_limits_color.b = 1.0;
00054   
00055   std_msgs::ColorRGBA point_markers;
00056   point_markers.a = 1.0;
00057   point_markers.r = 1.0;
00058   point_markers.g = .8;
00059 
00060   std_msgs::ColorRGBA color;
00061   visualization_msgs::MarkerArray arr;
00062   if(!state->areJointsWithinBounds(joint_names)) {
00063     color = joint_limits_color;
00064   } else if(collision_models.isKinematicStateInCollision(*state)) {
00065     color = collision_color;
00066     collision_models.getAllCollisionPointMarkers(*state,
00067                                                  arr,
00068                                                  point_markers,
00069                                                  ros::Duration(0.2));
00070   } else {
00071     color = good_color;
00072   }
00073 
00074   collision_models.getRobotMarkersGivenState(*state,
00075                                              arr,
00076                                              color,
00077                                              "right_arm",
00078                                              ros::Duration(0.2),
00079                                              &arm_names);
00080 
00081   while(ros::ok()) {    
00082     vis_marker_array_publisher_.publish(arr);
00083     ros::spinOnce();
00084     ros::Duration(.1).sleep();
00085   }
00086   collision_models.revertPlanningScene(state);
00087   ros::shutdown();
00088 }


pr2_arm_navigation_tutorials
Author(s): Sachin Chitta
autogenerated on Tue Apr 22 2014 19:29:47