00001 #include <ros/ros.h> 00002 #include <planning_environment_msgs/GetRobotState.h> 00003 #include <planning_environment_msgs/GetJointTrajectoryValidity.h> 00004 00005 int main(int argc, char **argv){ 00006 ros::init (argc, argv, "get_trajectory_validity_test"); 00007 ros::NodeHandle rh; 00008 00009 ros::service::waitForService("environment_server_right_arm/get_trajectory_validity"); 00010 planning_environment_msgs::GetJointTrajectoryValidity::Request req; 00011 planning_environment_msgs::GetJointTrajectoryValidity::Response res; 00012 ros::ServiceClient check_trajectory_validity_client_ = rh.serviceClient<planning_environment_msgs::GetJointTrajectoryValidity>("environment_server_right_arm/get_trajectory_validity"); 00013 00014 ros::service::waitForService("environment_server_right_arm/get_robot_state"); 00015 ros::ServiceClient get_state_client_ = rh.serviceClient<planning_environment_msgs::GetRobotState>("environment_server_right_arm/get_robot_state"); 00016 planning_environment_msgs::GetRobotState::Request request; 00017 planning_environment_msgs::GetRobotState::Response response; 00018 if(get_state_client_.call(request,response)) 00019 { 00020 req.robot_state = response.robot_state; 00021 } 00022 else 00023 { 00024 ROS_ERROR("Service call to get robot state failed on %s",get_state_client_.getService().c_str()); 00025 } 00026 00027 req.trajectory.joint_names.push_back("r_shoulder_pan_joint"); 00028 req.trajectory.points.resize(100); 00029 for(unsigned int i=0; i < 100; i++) 00030 { 00031 req.trajectory.points[i].positions.push_back(-(3*M_PI*i/4.0)/100.0); 00032 } 00033 req.check_collisions = true; 00034 if(check_trajectory_validity_client_.call(req,res)) 00035 { 00036 if(res.error_code.val == res.error_code.SUCCESS) 00037 ROS_INFO("Requested trajectory is not in collision"); 00038 else 00039 ROS_INFO("Requested trajectory is in collision. Error code: %d",res.error_code.val); 00040 } 00041 else 00042 { 00043 ROS_ERROR("Service call to check trajectory validity failed %s",check_trajectory_validity_client_.getService().c_str()); 00044 return false; 00045 } 00046 ros::shutdown(); 00047 }