trajectory filter response with nothing [closed]

edit

I've followed this tutorial

There seemed to be some out-dated information in this tutorial, so I modified some processes and did as follow :

roslaunch pr2_gazebo pr2_empty_world.launch

modify trajectory_filter_server/launch/trajectory_filter.launch so that it would load the config filter_unnormalize.yaml. Therefore, this node will provide a service server named /trajectory_filter_server/filter_trajectory with type arm_navigation_msgs::FilterJointTrajectory

Then fill in the service request following the tutorial, and the start_state additionally, since the warning from the server node indicates. After setting all this up, the service response still response nothing. I think the most important parameter returned is res.trajectory.points[i].velocities and res.trajectory.points[i].time_from_start, but the size() of the velocities vectors is zero, and the value of time_from_start is zero. How to use this service? Thanks~

Here is my source code :

#include "ros/ros.h"
#include "arm_navigation_msgs/FilterJointTrajectory.h"

const std::string FILTER_SERVER = "/trajectory_filter_server/filter_trajectory";

int main(int argc, char** argv)
{
    ros::init(argc, argv, "filter");
    ros::NodeHandle nh;

    ros::service::waitForService(FILTER_SERVER);
    ros::ServiceClient client = nh.serviceClient<arm_navigation_msgs::FilterJointTrajectory>(FILTER_SERVER);

    arm_navigation_msgs::FilterJointTrajectory::Request req;
    arm_navigation_msgs::FilterJointTrajectory::Response res;

    req.trajectory.joint_names.push_back("r_shoulder_pan_joint");
    req.trajectory.joint_names.push_back("r_shoulder_lift_joint");
    req.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
    req.trajectory.joint_names.push_back("r_elbow_flex_joint");
    req.trajectory.joint_names.push_back("r_forearm_roll_joint");
    req.trajectory.joint_names.push_back("r_wrist_flex_joint");
    req.trajectory.joint_names.push_back("r_wrist_roll_joint");

    req.trajectory.points.resize(3);
    for(unsigned int i = 0; i < 3; i++)
    {
        req.trajectory.points[i].positions.resize(7);
    }

    req.trajectory.points[1].positions[0] = -2.0;
    req.trajectory.points[2].positions[0] = 0.0;
    req.trajectory.points[1].positions[1] = 0.0;
    req.trajectory.points[2].positions[1] = 1.0;
    req.trajectory.points[1].positions[2] = -3.0;
    req.trajectory.points[2].positions[2] = 0.0;
    req.trajectory.points[1].positions[3] = -2.0;
    req.trajectory.points[2].positions[3] = -1.0;
    req.trajectory.points[1].positions[4] = -2.0;
    req.trajectory.points[2].positions[4] = 1.0;
    req.trajectory.points[1].positions[5] = -2.0;
    req.trajectory.points[2].positions[5] = -1.0;
    req.trajectory.points[1].positions[6] = -2.0;
    req.trajectory.points[2].positions[6] = 1.0;

    req.allowed_time = ros::Duration(3.0);

    req.start_state.joint_state.name = req.trajectory.joint_names;
    req.start_state.joint_state.position.resize(7);
    req.start_state.joint_state.position.push_back(0.0);
    req.start_state.joint_state.position.push_back(0.0);
    req.start_state.joint_state.position.push_back(0.0);
    req.start_state.joint_state.position.push_back(-0.468);
    req.start_state.joint_state.position.push_back(0.0);
    req.start_state.joint_state.position.push_back(-0.79);
    req.start_state.joint_state.position.push_back(0.0);

    for(unsigned int i = 0; i < 7; i++)
    {
        req.start_state.joint_state.velocity.push_back(0.0);
    }


    if(client.call(req, res))
    {
        ROS_INFO("Filter service state : %d", res.error_code.val);
    }

    ROS_INFO("DBG_size:%d", res.trajectory.points[2].velocities.size());
    ROS_INFO("DBG_time:%f", res.trajectory.points[1].time_from_start.toSec());

    for(unsigned int i = 0; i < 7; i++)
        ROS_INFO("DBG:%f", res.trajectory.points[1].velocities[i]);

    for(unsigned int i = 0; i < 7; i++)
    {
        ROS_INFO("%s : pos[0]=%f, vel[0]=%f, time_from_start[0]=%f ...