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