Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <arm_navigation_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 arm_navigation_msgs::FilterJointTrajectory::Request req;
00009 arm_navigation_msgs::FilterJointTrajectory::Response res;
00010 ros::ServiceClient filter_trajectory_client_ = rh.serviceClient<arm_navigation_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 }