test_forward_command_sine_node.cpp

Go to the documentation of this file.

/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */


#include <vector>
#include <string>

#include <ros/ros.h>
#include <std_msgs/Float64MultiArray.h>

class ForwardCommandExecutionTester
{
public:
    ForwardCommandExecutionTester()
    {
        dof_ = 2;
        idx_ = 1;

        output_pub_ = nh_.advertise<std_msgs::Float64MultiArray>("/torso/joint_group_position_controller/command", 1);

        ros::Duration(1.0).sleep();
    }


    ~ForwardCommandExecutionTester()
    {}

    void run()
    {
        ros::Rate r(100.0);

        ros::Time time = ros::Time::now();
        ros::Time start_time = time;
        double x = 0.0;

        double a = 0.6, b = 0.4, c = 0, d = 0;      // torso_2dof

        std_msgs::Float64MultiArray command_msg;
        command_msg.data.assign(dof_, 0.0);

        while (ros::ok())
        {
            time = ros::Time::now();
            x = (time - start_time).toSec();

            double vel = a*sin(b*x+c) + d;

            command_msg.data[idx_] = vel;

            output_pub_.publish(command_msg);

            ros::spinOnce();
            r.sleep();
        }
    }


    ros::NodeHandle nh_;
    ros::Publisher output_pub_;
    unsigned int dof_;
    unsigned int idx_;
};



int main(int argc, char **argv)
{
    ros::init(argc, argv, "test_forward_command_execution_node");

    ForwardCommandExecutionTester fcet;
    fcet.run();
    ros::spin();
    return 0;
}