joystick_teleop.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <sensor_msgs/Joy.h>
#include <geometry_msgs/Twist.h>
#include "bipedRobin_msgs/SetModeService.h"
#include "std_srvs/Empty.h"

/*
Dieser Knoten verarbeitet die joy_msg von joy/joy_node und erzeugt daraus eine geometry_msgs::Twist die x y und rotation um die Z achse and den bipedRobin_teleop/biped_teleop knoten weitergibt.
Weiters werden mit den Tasten auf dem Joypad verschiedene Services im bipedRobin_driver/biped_serializer Knoten aufgerufen
Und es kann die Freigabe zum gehen and den bipedRobin_navigation/navigation_master gesendet werden
*/

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
double vel_x, vel_y, vel_rot; // Geschwindigkeiten

void joyCallback( const sensor_msgs::Joy::ConstPtr& joy_msg )
{
        if( joy_msg->axes.size() < 3 )
        {
                ROS_ERROR( "Too few joystick axes: %d (expected: 3+)", joy_msg->axes.size() );
                return;
        }

        ROS_DEBUG("Received [axis0=%f axis1=%f axis2=%f]", joy_msg->axes[0], joy_msg->axes[1], joy_msg->axes[2] );
        
        if(joy_msg->buttons[4] == 1){   //L1 als Totmannschalter
                vel_x = joy_msg->axes[1] * max_speed_x;
                vel_y = joy_msg->axes[0] * max_speed_y;
        } else {
                vel_x = 0;
                vel_y = 0;
        }

        if(joy_msg->buttons[5] == 1){ //R1 als Totmannschalter
                vel_rot = joy_msg->axes[2] * max_rotation;
        } else {
                vel_rot = 0;
        }
}

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

        //publisher for cmd_vel and bipedRobin_teleop/biped_teleop
        ros::Publisher  vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);
        //subscriber for joy/joy_node
        ros::Subscriber joy_sub = n.subscribe( "joy", 10, &joyCallback );

        ros::Rate loop_rate(100); //publishen der Twist message mit 100 Hz
        while( ros::ok() ) 
        {
                geometry_msgs::Twist twist_msg;
                twist_msg.linear.x = vel_x;
                twist_msg.linear.y = vel_y;
                twist_msg.angular.z = vel_rot;

                if (1 || vel_x != 0 || vel_y != 0 || vel_rot != 0) {
                        vel_pub.publish(twist_msg);
                }

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