test_twist_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 <geometry_msgs/Twist.h>

class TwistCommandExecutionTester
{
public:
    TwistCommandExecutionTester()
    {
        output_pub_ = nh_.advertise<geometry_msgs::Twist>("command", 1);

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


    ~TwistCommandExecutionTester()
    {}

    void run()
    {
        ros::Rate r(50.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;

        geometry_msgs::Twist command_msg;

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

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

            command_msg.angular.z = vel;

            output_pub_.publish(command_msg);

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


    ros::NodeHandle nh_;
    ros::Publisher output_pub_;
};



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

    TwistCommandExecutionTester tcet;
    tcet.run();
    ros::spin();
    return 0;
}