biped_joystick_center.cpp

Go to the documentation of this file.

#include "ros/ros.h"
#include "sensor_msgs/Joy.h"
#include "geometry_msgs/Twist.h"
#include "biped_robin_msgs/SetModeService.h"
#include "std_srvs/Empty.h"
#include "std_msgs/String.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
ros::Publisher speech_pub;

void joyCallback( const sensor_msgs::Joy::ConstPtr& joy_msg )
{
        biped_robin_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);
        }
        if(joy_msg->axes[4] == 1){ // Analog Joystick --> talk
                std_msgs::String str;
                str.data = "Hello! I am a biped robot. I have two legs and can walk like a human. Do you want to see it?";
                speech_pub.publish(str);
        }
        if(joy_msg->axes[5] == 1){ // Analog Joystick --> talk
                std_msgs::String str;
                str.data = "Did you like it?";
                speech_pub.publish(str);
        }
        if(joy_msg->axes[4] == -1){ // Analog Joystick --> talk
                std_msgs::String str;
                str.data = "You are welcome!";
                speech_pub.publish(str);
        }
        if(joy_msg->axes[5] == -1){ // Analog Joystick --> talk
                std_msgs::String str;
                str.data = "Thank you for visiting me. I hope you enjoyed my demonstrations and will visit me again.";
                speech_pub.publish(str);
        }
}

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

        //subscriber for joy/joy_node
        ros::Subscriber joy_sub = n.subscribe( "joy", 10, &joyCallback );
        //advertiser for speech output
        speech_pub = n.advertise<std_msgs::String>("speech",5);
        //service clients for bipedRobin_driver/biped_serializer
        setMode_client = n.serviceClient<biped_robin_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() )
        {
                ros::spinOnce();

                loop_rate.sleep();
        }
}