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(argc > 1) {
00025
00026 std::stringstream s(argv[1]);
00027 bool add;
00028 s >> add;
00029
00030 if(add) {
00031
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 }