$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(!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 trajectory_msgs::JointTrajectory trajectory; 00039 trajectory.joint_names.push_back("r_shoulder_pan_joint"); 00040 trajectory.points.resize(100); 00041 00042 std::map<std::string, double> start_vals; 00043 state->getKinematicStateValues(start_vals); 00044 double start_angle = start_vals["r_shoulder_pan_joint"]; 00045 00046 double goal_angle = .9; 00047 if(argc > 1) { 00048 std::stringstream s(argv[1]); 00049 s >> goal_angle; 00050 } 00051 00052 for(unsigned int i=0; i < 100; i++) 00053 { 00054 trajectory.points[i].positions.push_back(start_angle+i*(goal_angle-start_angle)/100.0); 00055 } 00056 00057 std_msgs::ColorRGBA good_color, collision_color, joint_limits_color; 00058 good_color.a = collision_color.a = joint_limits_color.a = .8; 00059 00060 good_color.g = 1.0; 00061 collision_color.r = 1.0; 00062 joint_limits_color.b = 1.0; 00063 00064 std_msgs::ColorRGBA point_markers; 00065 point_markers.a = 1.0; 00066 point_markers.r = 1.0; 00067 point_markers.g = .8; 00068 00069 visualization_msgs::MarkerArray arr; 00070 00071 arm_navigation_msgs::ArmNavigationErrorCodes traj_code; 00072 std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> traj_codes; 00073 arm_navigation_msgs::Constraints empty_constraints; 00074 00075 std::map<std::string, double> state_vals; 00076 if(!collision_models.isJointTrajectoryValid(*state, 00077 trajectory, 00078 empty_constraints, 00079 empty_constraints, 00080 traj_code, 00081 traj_codes, 00082 false)) { 00083 std_msgs::ColorRGBA color; 00084 state_vals["r_shoulder_pan_joint"] = trajectory.points[traj_codes.size()-1].positions[0]; 00085 state->setKinematicState(state_vals); 00086 if(traj_code.val == traj_code.JOINT_LIMITS_VIOLATED) { 00087 color = joint_limits_color; 00088 } else { 00089 color = collision_color; 00090 collision_models.getAllCollisionPointMarkers(*state, 00091 arr, 00092 point_markers, 00093 ros::Duration(0.2)); 00094 } 00095 collision_models.getRobotMarkersGivenState(*state, 00096 arr, 00097 color, 00098 "right_arm_invalid", 00099 ros::Duration(0.2), 00100 &arm_names); 00101 } 00102 state_vals["r_shoulder_pan_joint"] = trajectory.points.back().positions[0]; 00103 state->setKinematicState(state_vals); 00104 collision_models.getRobotMarkersGivenState(*state, 00105 arr, 00106 good_color, 00107 "right_arm_goal", 00108 ros::Duration(0.2), 00109 &arm_names); 00110 00111 while(ros::ok()) { 00112 vis_marker_array_publisher_.publish(arr); 00113 ros::spinOnce(); 00114 ros::Duration(.1).sleep(); 00115 } 00116 collision_models.revertPlanningScene(state); 00117 ros::shutdown(); 00118 } 00119