Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <arm_navigation_msgs/SetPlanningSceneDiff.h>
00003 #include <arm_navigation_msgs/GetStateValidity.h>
00004 #include <planning_environment/models/collision_models.h>
00005 #include <planning_environment/models/model_utils.h>
00006
00007 static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";
00008 static const std::string GET_STATE_VALIDITY_NAME = "/planning_scene_validity_server/get_state_validity";
00009
00010 int main(int argc, char **argv){
00011 ros::init (argc, argv, "get_state_validity_test");
00012 ros::NodeHandle rh;
00013
00014 ros::Publisher vis_marker_publisher_;
00015 ros::Publisher vis_marker_array_publisher_;
00016
00017 vis_marker_publisher_ = rh.advertise<visualization_msgs::Marker>("state_validity_markers", 128);
00018 vis_marker_array_publisher_ = rh.advertise<visualization_msgs::MarkerArray>("state_validity_markers_array", 128);
00019
00020 ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME);
00021 ros::ServiceClient set_planning_scene_diff_client =
00022 rh.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff>(SET_PLANNING_SCENE_DIFF_NAME);
00023
00024 ros::ServiceClient get_state_validity_client =
00025 rh.serviceClient<arm_navigation_msgs::GetStateValidity>(GET_STATE_VALIDITY_NAME);
00026
00027 arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req;
00028 arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res;
00029
00030 if(!set_planning_scene_diff_client.call(planning_scene_req, planning_scene_res)) {
00031 ROS_WARN("Can't get planning scene");
00032 return -1;
00033 }
00034
00035 planning_environment::CollisionModels collision_models("robot_description");
00036 planning_models::KinematicState* state =
00037 collision_models.setPlanningScene(planning_scene_res.planning_scene);
00038
00039 std::vector<std::string> arm_names =
00040 collision_models.getKinematicModel()->getModelGroup("right_arm")->getUpdatedLinkModelNames();
00041 std::vector<std::string> joint_names =
00042 collision_models.getKinematicModel()->getModelGroup("right_arm")->getJointModelNames();
00043
00044 if(argc > 1) {
00045 std::stringstream s(argv[1]);
00046 double val;
00047 s >> val;
00048 std::map<std::string, double> nvalues;
00049 nvalues["r_shoulder_pan_joint"] = val;
00050
00051 state->setKinematicState(nvalues);
00052 }
00053
00054 arm_navigation_msgs::GetStateValidity::Request vreq;
00055 arm_navigation_msgs::GetStateValidity::Response vres;
00056
00057 vreq.group_name = "right_arm";
00058
00059 planning_environment::convertKinematicStateToRobotState(*state,
00060 ros::Time::now(),
00061 collision_models.getWorldFrameId(),
00062 vreq.robot_state);
00063 vreq.check_joint_limits = true;
00064 vreq.check_collisions = true;
00065
00066 if(!get_state_validity_client.call(vreq, vres)) {
00067 ROS_WARN("Can't get validity");
00068 collision_models.revertPlanningScene(state);
00069 return -1;
00070 }
00071
00072 std_msgs::ColorRGBA good_color, collision_color, joint_limits_color;
00073 good_color.a = collision_color.a = joint_limits_color.a = .8;
00074
00075 good_color.g = 1.0;
00076 collision_color.r = 1.0;
00077 joint_limits_color.b = 1.0;
00078
00079 std_msgs::ColorRGBA point_markers;
00080 point_markers.a = 1.0;
00081 point_markers.r = 1.0;
00082 point_markers.g = .8;
00083
00084 std_msgs::ColorRGBA color;
00085 visualization_msgs::MarkerArray arr;
00086 if(vres.error_code.val == arm_navigation_msgs::ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED) {
00087 color = joint_limits_color;
00088 } else if(vres.error_code.val == arm_navigation_msgs::ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) {
00089 planning_environment::getCollisionMarkersFromContactInformation(vres.contacts,
00090 collision_models.getWorldFrameId(),
00091 arr,
00092 point_markers,
00093 ros::Duration(0.2));
00094 color = collision_color;
00095 } else if(vres.error_code.val == arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS) {
00096 color = good_color;
00097 } else {
00098 ROS_WARN_STREAM("Other error code " << vres.error_code.val);
00099 }
00100
00101 collision_models.getRobotMarkersGivenState(*state,
00102 arr,
00103 color,
00104 "right_arm",
00105 ros::Duration(0.2),
00106 &arm_names);
00107
00108 while(ros::ok()) {
00109 vis_marker_array_publisher_.publish(arr);
00110 ros::spinOnce();
00111 ros::Duration(.1).sleep();
00112 }
00113 collision_models.revertPlanningScene(state);
00114 ros::shutdown();
00115 }