00001 #include <ros/ros.h> 00002 #include <motion_planning_msgs/FilterJointTrajectory.h> 00003 00004 int main(int argc, char **argv){ 00005 ros::init (argc, argv, "filter_joint_trajectory"); 00006 ros::NodeHandle rh; 00007 ros::service::waitForService("trajectory_filter/filter_trajectory"); 00008 motion_planning_msgs::FilterJointTrajectory::Request req; 00009 motion_planning_msgs::FilterJointTrajectory::Response res; 00010 ros::ServiceClient filter_trajectory_client_ = rh.serviceClient<motion_planning_msgs::FilterJointTrajectory>("trajectory_filter/filter_trajectory"); 00011 00012 req.trajectory.joint_names.push_back("r_shoulder_pan_joint"); 00013 req.trajectory.joint_names.push_back("r_shoulder_lift_joint"); 00014 req.trajectory.points.resize(3); 00015 00016 for(unsigned int i=0; i < 3; i++) 00017 { 00018 req.trajectory.points[i].positions.resize(2); 00019 } 00020 00021 req.trajectory.points[1].positions[0] = 0.5; 00022 req.trajectory.points[2].positions[0] = -1.5; 00023 00024 req.trajectory.points[1].positions[1] = 0.2; 00025 req.trajectory.points[2].positions[1] = -0.5; 00026 00027 req.allowed_time = ros::Duration(1.0); 00028 00029 if(filter_trajectory_client_.call(req,res)) 00030 { 00031 if(res.error_code.val == res.error_code.SUCCESS) 00032 { 00033 ROS_INFO("Requested trajectory was filtered"); 00034 for(unsigned int i=0; i < res.trajectory.points.size(); i++) 00035 { 00036 ROS_INFO_STREAM(res.trajectory.points[i].positions[0] << "," << res.trajectory.points[i].velocities[0] << "," << res.trajectory.points[i].positions[1] << "," << res.trajectory.points[i].velocities[1] << "," << res.trajectory.points[i].time_from_start.toSec()); 00037 } 00038 } 00039 else 00040 ROS_INFO("Requested trajectory was not filtered. Error code: %d",res.error_code.val); 00041 } 00042 else 00043 { 00044 ROS_ERROR("Service call to filter trajectory failed %s",filter_trajectory_client_.getService().c_str()); 00045 } 00046 ros::shutdown(); 00047 }