biped_joystickCenter.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
*/
ros::ServiceClient acknowledge_client; //zum Aufrufen des Acknowledge services
ros::ServiceClient setMode_client; //zum Aufrufen des setMode services
ros::ServiceClient stop_client; //zum Aufrufen des stop services
ros::ServiceClient walk_client; //zum Aufrufen des stop services

void joyCallback( const sensor_msgs::Joy::ConstPtr& joy_msg )
{
        bipedRobin_msgs::SetModeService setMode; //Nachrichten für services
        std_srvs::Empty empty;

        if(joy_msg->buttons[0] == 1){   //Knopf 1 setServiceMode 0
                setMode.request.desiredMode = 0;
                setMode_client.call(setMode);
        }

        if(joy_msg->buttons[1] == 1){   //Knopf 2 setServiceMode 2
                setMode.request.desiredMode = 2;
                setMode_client.call(setMode);
        }

        if(joy_msg->buttons[2] == 1){   //Knopf 3 setServiceMode 3
                setMode.request.desiredMode = 3;
                setMode_client.call(setMode);
        }

        if(joy_msg->buttons[3] == 1){   //Knopf 4 walk_srv
                walk_client.call(empty);
        }

        if(joy_msg->buttons[8] == 1){   //Knopf SE acknowledge service
                acknowledge_client.call(empty);
        }

        if(joy_msg->buttons[9] == 1){   //Knopf ST stop service
                stop_client.call(empty);
        }
}

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

        //subscriber for joy/joy_node
        ros::Subscriber joy_sub = n.subscribe( "joy", 10, &joyCallback );
        //service clients for bipedRobin_driver/biped_serializer
        setMode_client = n.serviceClient<bipedRobin_msgs::SetModeService>("setMode_srv");
        acknowledge_client = n.serviceClient<std_srvs::Empty>("acknowledge_srv");
        stop_client = n.serviceClient<std_srvs::Empty>("stop_srv");
        //service client fo bipedRobin_navigation/navigation_master,
        walk_client = n.serviceClient<std_srvs::Empty>("walk_srv");

        ros::Rate loop_rate(100);
        while( ros::ok() ) //publishen der Twist message mit 20 Hz
        {
                ros::spinOnce();

                loop_rate.sleep();
        }
}