biped_teleop.cpp

Go to the documentation of this file.

#include "ros/ros.h"
#include "visualization_msgs/MarkerArray.h"
#include "visualization_msgs/Marker.h"
#include "geometry_msgs/PoseStamped.h"
#include "tf/transform_broadcaster.h"
#include "tf/transform_listener.h"
#include "biped_robin_msgs/StepTarget3DService.h"
#include "std_srvs/Empty.h"
#include "std_msgs/UInt8.h"
#include <math.h>

/*
Dieser Knoten empfängt eine Geschwindigkeitsvorgabe von bipedRobin_teleop/biped_joy_node, und erzeugt daraus Schritte.
Schritt werden nur gesendet wenn der Schrittbuffer im Roboter leer ist.
Die Anzahl der Schritt im Buffer wird vom bipedRobin_driver/biped_deserializer empfangen
Die auszuführen Schritte werden an bipedRobin_driver/biped_serializer gesendet
*/

#define DEF_LEG_DISTANCE 0.25
#define MIN_LEG_DISTANCE 0.22
#define MAX_LEG_DISTANCE 0.3

#define MAX_STEP_LENGTH 0.1
#define MAX_STEP_ANGLE 3.1415/8

ros::ServiceClient footstepInc_client; //sends Steps to the serializer
ros::Publisher goalVizPub;

geometry_msgs::PoseStamped goalViz;

int currentLegToMove = 1; //0.. right leg, 1.. left leg
bool sendStep = true; //Flag if we shoud send a step at time sendStepTime
int wasWalking = false; //flag that says if the robot was previously walking to know when to make the last step

ros::Time lastCallbackTime;
ros::Time thisCallbackTime;
ros::Time sendStepTime;

//TODO: use TF to get current position of the robot
tf::Transform desiredCenterPoint (tf::createQuaternionFromYaw(0), tf::Point(0, 0.271/2, 0));
tf::Transform desiredLeftLeg (tf::createQuaternionFromYaw(0), tf::Point(0, 0.271, 0));
tf::Transform desiredRightLeg (tf::createQuaternionFromYaw(0), tf::Point(0, 0, 0));

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

void visualize() {
        goalViz.header.seq++;
        goalViz.header.stamp = thisCallbackTime;
        goalViz.header.frame_id = "odom";

        goalViz.pose.position.x = desiredCenterPoint.getOrigin().x(); //desiredCenterPoint
        goalViz.pose.position.y = desiredCenterPoint.getOrigin().y();
        goalViz.pose.position.z = desiredCenterPoint.getOrigin().z();

        goalViz.pose.orientation.x = desiredCenterPoint.getRotation().getX();
        goalViz.pose.orientation.y = desiredCenterPoint.getRotation().getY();
        goalViz.pose.orientation.z = desiredCenterPoint.getRotation().getZ();
        goalViz.pose.orientation.w = desiredCenterPoint.getRotation().getW();

        goalVizPub.publish(goalViz);
}


void twistCallback(const geometry_msgs::Twist twist) {
        biped_robin_msgs::StepTarget3DService srv;

        thisCallbackTime = ros::Time::now();

        // update desired center point
        double deltaTime = 0.01;
        tf::Pose deltaCenterPoint (tf::createQuaternionFromYaw(twist.angular.z * deltaTime), tf::Point(twist.linear.x * deltaTime, twist.linear.y * deltaTime, 0.0));
        desiredCenterPoint = desiredCenterPoint * deltaCenterPoint;

        /*// TODO: saturate desired Center Point
        tf::Transform deltaStanceCenter;
        if (currentLegToMove == 0) { //right leg
                deltaStanceCenter = desiredCenterPoint.inverse() * desiredLeftLeg;
        } else { //left leg
                deltaStanceCenter = desiredCenterPoint.inverse() * desiredRightLeg;
        }
        double deltaX = deltaStanceCenter.getOrigin().getX();
        double deltaY = deltaStanceCenter.getOrigin().getY();
        double deltaG = tf::getYaw(deltaStanceCenter.getRotation());
        ROS_INFO("%f %f %f",deltaX,deltaY,deltaG);*/



        if (sendStep && (thisCallbackTime - sendStepTime).toSec() > 0 && (twist.linear.x != 0 || twist.linear.y != 0 || twist.angular.z != 0 || wasWalking > 0)) { //sendStep flag is set and velocity is nonzero or is first time zero  --> calculate next step and send it

                //figure out the next swing leg position and the relativ step
                tf::Transform deltaCenterSwing;
                tf::Transform step;
                if (currentLegToMove == 0) { //right leg
                        deltaCenterSwing.setOrigin(tf::Point(0.0, -DEF_LEG_DISTANCE/2, 0.0));
                        deltaCenterSwing.setRotation(tf::createQuaternionFromYaw(0.0));
                        desiredRightLeg = desiredCenterPoint * deltaCenterSwing;   //place swing leg next to the desired center point
                        step = desiredLeftLeg.inverse() * desiredRightLeg;
                } else { //left leg
                        deltaCenterSwing.setOrigin(tf::Point(0.0, DEF_LEG_DISTANCE/2, 0.0));
                        deltaCenterSwing.setRotation(tf::createQuaternionFromYaw(0.0));
                        desiredLeftLeg = desiredCenterPoint * deltaCenterSwing;   //place swing leg next to the desired center point
                        step = desiredRightLeg.inverse() * desiredLeftLeg;
                }

                //check if the step is feasible
                double deltaX = step.getOrigin().x();
                double deltaY = step.getOrigin().y();
                double deltaG = tf::getYaw(step.getRotation());

                deltaX = sgn(deltaX) * fmin(fabs(deltaX), MAX_STEP_LENGTH);
                deltaY = sgn(deltaY) * fmax(fmin(fabs(deltaY), MAX_LEG_DISTANCE), MIN_LEG_DISTANCE);
                deltaG = sgn(deltaG) * fmin(fabs(deltaG), MAX_STEP_ANGLE);

                tf::Pose satStep (tf::createQuaternionFromYaw(deltaG), tf::Point(deltaX, deltaY, 0.0));
                if (currentLegToMove == 0) { //right leg
                        desiredRightLeg = desiredLeftLeg * satStep;
                        desiredCenterPoint = desiredRightLeg * deltaCenterSwing.inverse();
                } else { //left leg
                        desiredLeftLeg = desiredRightLeg * satStep;
                        desiredCenterPoint = desiredLeftLeg * deltaCenterSwing.inverse();
                }

                // set the data for the step service and call it
                srv.request.step.pose.position.x = satStep.getOrigin().getX();
                srv.request.step.pose.position.y = satStep.getOrigin().getY();
                srv.request.step.pose.position.z = satStep.getOrigin().getZ();
                srv.request.step.pose.orientation.x = satStep.getRotation().getX();
                srv.request.step.pose.orientation.y = satStep.getRotation().getY();
                srv.request.step.pose.orientation.z = satStep.getRotation().getZ();
                srv.request.step.pose.orientation.w = satStep.getRotation().getW();
                srv.request.step.leg = currentLegToMove;
                footstepInc_client.call(srv); //call service

                // update next leg to move
                if (currentLegToMove == 0) {
                        currentLegToMove = 1;
                } else {
                        currentLegToMove = 0;
                }

                // check if this was the last step and the robot should move his legs parallel
                if (wasWalking <= 0) wasWalking = 2; //we need to final steps
                if (twist.linear.x == 0 && twist.linear.y == 0 && twist.angular.z == 0) wasWalking--;

                sendStep = false; //wait till next step is asked by robot
        }

        visualize();

        lastCallbackTime = ros::Time::now();
}

void stepsLeftCallback(std_msgs::UInt8 stepsLeftInBuffer) {
        if(stepsLeftInBuffer.data == 0){ //keine Schritte mehr im Buffer
                //ros::Duration(0.8).sleep(); //warten bevor neuer Schritt gesendet wird
                sendStep = true;
                sendStepTime = ros::Time::now() + ros::Duration(0.8);
        }
}



int main(int argc, char **argv)
{
  /* init ROS */

        ros::init(argc, argv, "biped_teleop");

        ros::NodeHandle n;
        ros::NodeHandle pn("~");

        //client zum senden der Schritt an den serializer
        footstepInc_client = n.serviceClient<biped_robin_msgs::StepTarget3DService>("footstep3DInc_srv");
        //subscriber zum Einlesen der Anzahl der Schritte im Buffer
        ros::Subscriber stepsLeftInBuffer_sub = n.subscribe<std_msgs::UInt8>("stepsLeftInBuffer", 1, stepsLeftCallback);
        //subscriber zum Einlesen der geschwindigkeit vom biped_joy_node
        ros::Subscriber twist = n.subscribe<geometry_msgs::Twist>("/cmd_vel", 1, twistCallback);

        goalVizPub = pn.advertise<geometry_msgs::PoseStamped>("desiredCenterPoint", 10);

        //reste visualization
        ros::Duration(0.5).sleep();
        visualize();

        ros::spin();

        return 0;
}