carrot_teleop.cpp

Go to the documentation of this file.

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "tf/transform_listener.h"

//This node looks up a transform and then uses this transform to generate a cmd_vel for the robot so that the robot follows the transform

const double max_speed_x = 0.1; // m/s
const double max_speed_y = 0.05; // m/s
const double max_rotation = 0.2; // rad/s
const double minimal_distance_x = 0.1;
const double minimal_distance_y = 0.2;

template <typename T> int sgn(T val) {
    return (T(0) < val) - (val < T(0));
}

int main(int argc, char **argv) {
        ros::init(argc, argv, "biped_goalpose");
        ros::NodeHandle pn ("~");
        
        tf::TransformListener tfListener;

        ros::Publisher pub = pn.advertise<geometry_msgs::Twist>("cmd_vel", 10);

        std::string carrotFrame = "/carrot";
        std::string baseFrame = "/base_footprint";
        double carrot_distance_x;
        pn.param("carrot_frame_id", carrotFrame, carrotFrame);
        pn.param("base_frame_id", baseFrame, baseFrame);
        pn.param("carrot_distance_x", carrot_distance_x, -0.6);
        
        ros::Rate loop_rate(100.0); //publishen the Twist message with 100 Hz
        
        while (pn.ok()) {
                tf::StampedTransform transform;
                double dx, dy, da;
                
                ros::Time now = ros::Time::now();
                
                try {
                        tfListener.waitForTransform(baseFrame, carrotFrame, now-ros::Duration(0.5), ros::Duration(0.5)); //makes sure that the carrot was detected in the last 0.5s
                        tfListener.lookupTransform(baseFrame, carrotFrame, ros::Time(0), transform); //uses the last available carrot transform
                        dx = transform.getOrigin().x() + carrot_distance_x;
                        dy = transform.getOrigin().y();
                        da = atan2(dy, dx);
                } catch (tf::TransformException ex) {
                        //ROS_ERROR("%s",ex.what());
                        ROS_INFO("Carrot not found! Please move the carrot into the field of view of the robot!");
                        dx = 0;
                        dy = 0;
                        da = 0;
                }

                ROS_INFO("Carrot is at position  x:%f, y:%f", dx, dy);
                
                if (fabs(dx) < minimal_distance_x) dx = 0;
                if (fabs(dy) < minimal_distance_y) dy = 0;
                da = atan2(dy, dx);
                
                geometry_msgs::Twist cmd_vel;
                cmd_vel.linear.x = sgn(dx) * fmin(fabs(dx)*0.5, max_speed_x);
                cmd_vel.linear.y = sgn(dy) * fmin(fabs(dy)*0.5, max_speed_y)*0;
                cmd_vel.angular.z = sgn(da) * fmin(fabs(da), max_rotation);
                
                pub.publish(cmd_vel);
                
                loop_rate.sleep();
                ros::spinOnce();
        }

        return 0;
}