$search
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 SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff"; 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(SET_PLANNING_SCENE_DIFF_NAME); 00018 ros::ServiceClient get_planning_scene_client = 00019 rh.serviceClient<arm_navigation_msgs::GetPlanningScene>(SET_PLANNING_SCENE_DIFF_NAME); 00020 00021 arm_navigation_msgs::GetPlanningScene::Request planning_scene_req; 00022 arm_navigation_msgs::GetPlanningScene::Response planning_scene_res; 00023 00024 if(argc > 1) { 00025 00026 std::stringstream s(argv[1]); 00027 bool add; 00028 s >> add; 00029 00030 if(add) { 00031 //add the cylinder into the collision space 00032 arm_navigation_msgs::CollisionObject cylinder_object; 00033 cylinder_object.id = "pole"; 00034 cylinder_object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD; 00035 cylinder_object.header.frame_id = "odom_combined"; 00036 cylinder_object.header.stamp = ros::Time::now(); 00037 arm_navigation_msgs::Shape object; 00038 object.type = arm_navigation_msgs::Shape::CYLINDER; 00039 object.dimensions.resize(2); 00040 object.dimensions[0] = .1; 00041 object.dimensions[1] = 1.0; 00042 geometry_msgs::Pose pose; 00043 pose.position.x = .6; 00044 pose.position.y = -.6; 00045 pose.position.z = .5; 00046 pose.orientation.x = 0; 00047 pose.orientation.y = 0; 00048 pose.orientation.z = 0; 00049 pose.orientation.w = 1; 00050 cylinder_object.shapes.push_back(object); 00051 cylinder_object.poses.push_back(pose); 00052 00053 planning_scene_req.planning_scene_diff.collision_objects.push_back(cylinder_object); 00054 } 00055 } 00056 00057 if(!get_planning_scene_client.call(planning_scene_req, planning_scene_res)) { 00058 ROS_WARN("Can't get planning scene"); 00059 return -1; 00060 } 00061 00062 planning_environment::CollisionModels collision_models("robot_description"); 00063 planning_models::KinematicState* state = 00064 collision_models.setPlanningScene(planning_scene_res.planning_scene); 00065 00066 std::vector<std::string> arm_names = 00067 collision_models.getKinematicModel()->getModelGroup("right_arm")->getUpdatedLinkModelNames(); 00068 std::vector<std::string> joint_names = 00069 collision_models.getKinematicModel()->getModelGroup("right_arm")->getJointModelNames(); 00070 00071 std::map<std::string, double> nvalues; 00072 nvalues["r_shoulder_lift_joint"] = .9; 00073 00074 if(argc > 2) { 00075 std::stringstream s(argv[2]); 00076 double val; 00077 s >> val; 00078 nvalues["r_shoulder_pan_joint"] = val; 00079 } 00080 state->setKinematicState(nvalues); 00081 00082 00083 std_msgs::ColorRGBA good_color, collision_color, joint_limits_color; 00084 good_color.a = collision_color.a = joint_limits_color.a = .8; 00085 00086 good_color.g = 1.0; 00087 collision_color.r = 1.0; 00088 joint_limits_color.b = 1.0; 00089 00090 std_msgs::ColorRGBA point_markers; 00091 point_markers.a = 1.0; 00092 point_markers.r = 1.0; 00093 point_markers.g = .8; 00094 00095 std_msgs::ColorRGBA color; 00096 visualization_msgs::MarkerArray arr; 00097 if(!state->areJointsWithinBounds(joint_names)) { 00098 color = joint_limits_color; 00099 } else if(collision_models.isKinematicStateInCollision(*state)) { 00100 color = collision_color; 00101 collision_models.getAllCollisionPointMarkers(*state, 00102 arr, 00103 point_markers, 00104 ros::Duration(0.2)); 00105 } else { 00106 color = good_color; 00107 } 00108 00109 collision_models.getRobotMarkersGivenState(*state, 00110 arr, 00111 color, 00112 "right_arm", 00113 ros::Duration(0.2), 00114 &arm_names); 00115 00116 while(ros::ok()) { 00117 vis_marker_array_publisher_.publish(arr); 00118 ros::spinOnce(); 00119 ros::Duration(.1).sleep(); 00120 } 00121 collision_models.revertPlanningScene(state); 00122 ros::shutdown(); 00123 }