humotion

PROJECT(meka_humotion_server)

cmake_minimum_required(VERSION 2.8)

SET(MAIN ${PROJECT_NAME})

set(ENV{ROS_LANG_DISABLE} "genjava")

set(ROS_BUILD_TYPE Debug)

################################################################

# check for ROS support:

find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs message_generation genmsg)

IF (catkin_FOUND)

    set(ROS_FOUND 1)

    message(STATUS "ROS Support is ON")

    add_definitions(-DROS_SUPPORT=1)

ENDIF (catkin_FOUND)

IF (NOT catkin_FOUND)

  message(FATAL_ERROR "Error: could not find ROS!")

ENDIF ()

FIND_PACKAGE(Boost REQUIRED COMPONENTS system thread)

FIND_PACKAGE(humotion)

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")

#add_message_files(

#    FILES

#)

#generate_messages(

#    DEPENDENCIES

#    std_msgs

#)

#hack to allow sub dir calls work

catkin_package(

    INCLUDE_DIRS include

    LIBRARIES humotion

    #CATKIN_DEPENDS message_runtime

    #DEPENDS system_lib

)

include_directories( ${catkin_INCLUDE_DIRS})

# add include directories

INCLUDE_DIRECTORIES(${YARP_INCLUDE_DIRS} ${ICUB_INCLUDE_DIRS} ${humotion_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} include/)

link_directories(${Boost_LIBRARY_DIRS} ${humotion_LIBRARY_DIRS})

# add required linker flags

SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${ICUB_LINK_FLAGS}")

file(GLOB DUMMY_HEADER_LIST RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} include/*.h)

#add_dependencies(${MAIN} ${catkin_EXPORTED_TARGETS})

ADD_EXECUTABLE(${MAIN} src/main.cpp src/mekajointinterface.cpp ${DUMMY_HEADER_LIST})

TARGET_LINK_LIBRARIES(${MAIN} ${Boost_LIBRARIES} ${catkin_LIBRARIES} ${humotion_LIBRARIES})

set_property(TARGET ${MAIN} PROPERTY INSTALL_RPATH_USE_LINK_PATH TRUE)

#INSTALL(TARGETS ${MAIN} DESTINATION bin)

install(TARGETS ${MAIN}

    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

    RUNTIME DESTINATION bin 

)

#pragma once

#include <humotion/server/joint_interface.h>

#include <humotion/server/server.h>

#include <boost/bimap.hpp>

//#include "meka_data_receiver.h"

//class MekaDataReceiver;

#include "ros/ros.h"

#include "sensor_msgs/JointState.h"

#include "trajectory_msgs/JointTrajectory.h"

#include <m3meka_msgs/M3ControlStates.h>

class MekaJointInterface : public humotion::server::JointInterface{

public:

    MekaJointInterface(std::string _input_scope, std::string control_scope, std::string _output_scope);

    ~MekaJointInterface();

    //void fetch_position(Device *dev, double timestamp);

    //void fetch_speed(Device *dev, double timestamp);

    //void fetch_position(int id, double value, double timestamp);

    //void fetch_speed(int id, double value, double timestamp);

    void run();

    static const int MAIN_LOOP_FREQUENCY = 50;

protected:

    void disable_joint(int e);

    void publish_target(int e, float position, float velocity);

    void enable_joint(int e);

    void execute_motion();

private:

    void incoming_controlstate(const m3meka_msgs::M3ControlStates &control_state);

    void incoming_jointstates(const sensor_msgs::JointState & msg);

    void store_dummy_data(int id, double timestamp);

    void store_min_max(int id, float min, float max);

    ros::Subscriber joint_state_subscriber;

    ros::Subscriber control_state_subscriber;

    ros::Publisher target_publisher;

    double get_timestamp_s();

    void set_eyelid_angle(double angle);

    void set_eyebrow_angle(int id);

    void set_mouth();

    //iCubDataReceiver *icub_data_receiver;

    void init_joints();

    double lid_angle;

    int lid_opening_previous;

    int previous_mouth_state;

    std::string input_scope;

    std::string control_scope;

    std::string output_scope;

    float last_pos_eye_vergence;

    float last_pos_eye_pan;

    float last_vel_eye_vergence;

    float last_vel_eye_pan;

    bool controller_enabled;

    void store_joint(int id, float value);

    void set_target_in_positionmode(int id, double value);

    void set_target_in_velocitymode(int id, double value);

    int convert_enum_to_motorid(int e);

    int convert_motorid_to_enum(int id);

    typedef boost::bimap<int, int > enum_id_bimap_t;

    typedef enum_id_bimap_t::value_type enum_id_bimap_entry_t;

    enum_id_bimap_t enum_id_bimap;

};

<?xml version="1.0"?>

<package>

  <name>meka_humotion_server</name>

  <version>0.0.1</version>

  <description>humotion server for the mekabot</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag --> 

  <maintainer email="sschulz@todo.todo">Simon Schulz</maintainer>

  <!-- One license tag required, multiple allowed, one license per tag -->

  <!-- Commonly used license strings: -->

  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->

  <license>TODO</license>

  <!-- Url tags are optional, but mutiple are allowed, one per tag -->

  <!-- Optional attribute type can be: website, bugtracker, or repository -->

  <!-- Example: -->

  <url type="website">https://projects.cit-ec.uni-bielefeld.de/git/flobidev.core.git</url> -->

  <!-- Author tags are optional, mutiple are allowed, one per tag -->

  <!-- Authors do not have to be maintianers, but could be -->

  <!-- Example: -->

  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->

  <!-- The *_depend tags are used to specify dependencies -->

  <!-- Dependencies can be catkin packages or system dependencies -->

  <!-- Examples: -->

  <!-- Use build_depend for packages you need at compile time: -->

  <!-- Use buildtool_depend for build tool packages: -->

  <!--   <buildtool_depend>catkin</buildtool_depend> -->

  <!-- Use run_depend for packages you need at runtime: -->

  <!-- <run_depend>message_runtime</run_depend> -->

  <!-- Use test_depend for packages you need only for testing: -->

  <!--   <test_depend>gtest</test_depend> -->

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>roscpp</build_depend>

  <build_depend>std_msgs</build_depend>

  <!-- <run_depend>roscpp</run_depend> -->

  <!-- <run_depend>std_msgs</run_depend> -->

  <!--<build_depend>actionlib</build_depend>

  <build_depend>actionlib_msgs</build_depend>

  <run_depend>actionlib</run_depend> -->

  <!-- <run_depend>actionlib_msgs</run_depend> -->

  <!-- The export tag contains other, unspecified, tags -->

  <export>

    <!-- You can specify that this package is a metapackage here: -->

    <!-- <metapackage/> -->

    <!-- Other tools can request additional information be placed here -->

  </export>

</package>

#include <stdio.h>

#include <humotion/server/server.h>

#include <string>

#include <iostream>

//#include "meka_data_receiver.h"

#include "mekajointinterface.h"

using namespace std;

int main(int argc, char *argv[]){

    /*Property params;

    params.fromCommand(argc, argv);

    if (!params.check("robot")){

        fprintf(stderr, "Please specify the name of the robot\n");

        fprintf(stderr, "--robot name (e.g. icub or icubSim)\n");

        return -1;

    }

    string robotName=params.find("robot").asString().c_str();

    string scope="/"+robotName;

*/

    if (argc != 5){

        printf("> ERROR: invalid number of parameters passed to server!\n\n");

        printf("usage   : %s <humotion base topic> <jointstates topic> <controlstates topic> <control output topic>\n\n",argv[0]);

        printf("example : %s /meka /joint_states  /meka_roscontrol_state_manager/state /meka_roscontrol/head_position_trajectory_controller/command)\n\n",argv[0]);

        exit(EXIT_FAILURE);

    }

    //create humotion interface

    string humotion_scope      = argv[1];

    string jointstates_scope   = argv[2];

    string controlstates_scope = argv[3];

    string control_scope       = argv[4];

    MekaJointInterface *jointinterface = new MekaJointInterface(jointstates_scope, controlstates_scope, control_scope);

    humotion::server::Server *humotion_server = new humotion::server::Server(humotion_scope, "ROS", jointinterface);

    //finally run it

    jointinterface->run();

    while(humotion_server->ok()){

        usleep(100000);

    }

    printf("> ros connection died, will exit now\n");

    return 0;

}

#include "mekajointinterface.h"

using namespace std;

//WARNING: DO NOT CHANGE THIS; VELOCITYMODE IS NOT YET IMPLEMENTED

#define POSITION_CONTROL 1

void MekaJointInterface::incoming_controlstate(const m3meka_msgs::M3ControlStates &control_state){

    //incoming controller status

    for (unsigned int i = 0; i<control_state.group_name.size(); i++){

        if (control_state.group_name[i] == "head"){

            //enable/disable based on controller status

            if (control_state.state[i] == m3meka_msgs::M3ControlStates::START){

                //controller up and running

                if (!controller_enabled){

                    printf("> incoming control state (%d), enabling jointstate output\n",control_state.state[i]);

                    controller_enabled = true;

                }

            }else{

                //controller in estop/stopped etc

                if (controller_enabled){

                    printf("> incoming control state (%d), DISABLING jointstate output\n",control_state.state[i]);

                    controller_enabled = false;

                }

            }

        }

    }

}

void MekaJointInterface::incoming_jointstates(const sensor_msgs::JointState & msg){

    //fetch current timestamp

    double timestamp = msg.header.stamp.toSec();

    //iterate through incoming joints and filter out joints we need:

    for(int i=0; i<msg.name.size(); i++){

        string name = msg.name[i];

        //printf("incoming data for joint '%s'\n", name.c_str());

        int id = -1;

        if (name == "head_j1"){

            id = ID_NECK_PAN;

        }else if(name == "head_j0"){

            id = ID_NECK_TILT;

        }

        //store data:

        if (id != -1){

            //printf("> storing joint data for joint id %d\n", id);

            if (i >= msg.position.size()){

                printf("> joint state msg is missing position data for joint '%s'...\n", name.c_str());

                return;

            }

            if (i >= msg.velocity.size()){

                //printf("> joint state msg is missing velocity data for joint '%s'...\n", name.c_str());

                //exit(EXIT_FAILURE);

                return;

            }

            //ok, safe to access data

            if (id == ID_NECK_PAN){

                //joint is inverted

                JointInterface::store_incoming_position(id, -180.0 / M_PI * msg.position[i], timestamp);

                JointInterface::store_incoming_speed(   id, -180.0 / M_PI * msg.velocity[i], timestamp);

            }else if (id == ID_NECK_TILT){

                JointInterface::store_incoming_position(id, 180.0 / M_PI * msg.position[i], timestamp);

                JointInterface::store_incoming_speed(   id, 180.0 / M_PI * msg.velocity[i], timestamp);

            }

        }

    }

    //dummy data uses current time

    timestamp = get_timestamp_s();

    //store dummy positions for joints we do not know about:

    store_dummy_data(ID_LIP_LEFT_UPPER, timestamp);

    store_dummy_data(ID_LIP_LEFT_LOWER, timestamp);

    store_dummy_data(ID_LIP_CENTER_UPPER, timestamp);

    store_dummy_data(ID_LIP_CENTER_LOWER, timestamp);

    store_dummy_data(ID_LIP_RIGHT_UPPER, timestamp);

    store_dummy_data(ID_LIP_RIGHT_LOWER, timestamp);

    store_dummy_data(ID_NECK_ROLL, timestamp);

    store_dummy_data(ID_EYES_BOTH_UD, timestamp);

    store_dummy_data(ID_EYES_LEFT_LR, timestamp);

    store_dummy_data(ID_EYES_RIGHT_LR, timestamp);

    store_dummy_data(ID_EYES_LEFT_LID_LOWER, timestamp);

    store_dummy_data(ID_EYES_LEFT_LID_UPPER, timestamp);

    store_dummy_data(ID_EYES_LEFT_BROW, timestamp);

    store_dummy_data(ID_EYES_RIGHT_LID_LOWER, timestamp);

    store_dummy_data(ID_EYES_RIGHT_LID_UPPER, timestamp);

    store_dummy_data(ID_EYES_RIGHT_BROW, timestamp);

}

void MekaJointInterface::store_dummy_data(int id, double timestamp){

    JointInterface::store_incoming_position(id, 0.0, timestamp);

    JointInterface::store_incoming_speed(id, 0.0, timestamp);

}

//! constructor

MekaJointInterface::MekaJointInterface(string _input_scope, string control_scope, string _output_scope) : humotion::server::JointInterface(){

    input_scope = _input_scope;

    output_scope = _output_scope;

    controller_enabled = false;

    //subscribe to meka joint states

    int argc = 0;

    ros::init(argc, (char**)NULL, "meka_humotion");

    ros::NodeHandle n;

    printf("> listening on jointstates on '%s'\n",input_scope.c_str());

    joint_state_subscriber = n.subscribe(input_scope, 150, &MekaJointInterface::incoming_jointstates, this);

    printf("> listening on controlstates on '%s'\n",control_scope.c_str());

    control_state_subscriber = n.subscribe(control_scope, 150, &MekaJointInterface::incoming_controlstate, this);

    printf("> sending targets on '%s'\n", output_scope.c_str());

    target_publisher = n.advertise<trajectory_msgs::JointTrajectory>(output_scope, 100);

    //tell humotion about min/max joint values:

    init_joints();

}

//! destructor

MekaJointInterface::~MekaJointInterface(){

}

void MekaJointInterface::run(){

   ros::spin();

}

double MekaJointInterface::get_timestamp_s(){

    struct timespec spec;

    clock_gettime(CLOCK_REALTIME, &spec);

    return spec.tv_sec + spec.tv_nsec / 1.0e9;

}

//! set the target position of a joint

//! \param enum id of joint

//! \param float value

void MekaJointInterface::publish_target(int e, float position, float velocity){

   //we do this in execute motion for all joints at once...

}

//! actually execute the scheduled motion commands

void MekaJointInterface::execute_motion(){

    if (controller_enabled){

        //build msg

        trajectory_msgs::JointTrajectory msg;

        msg.joint_names.push_back("head_j0");

        msg.joint_names.push_back("head_j1");

        trajectory_msgs::JointTrajectoryPoint p;

        p.positions.push_back(get_target_position(ID_NECK_TILT) * M_PI / 180.0);

        //pan joint is inverted!

        p.positions.push_back(-get_target_position(ID_NECK_PAN) * M_PI / 180.0);

        //printf("targets pan=%4.1f tilt=%4.1f (eye p %4.1f t %4.2f)\n",joint_target[ID_NECK_TILT],joint_target[ID_NECK_PAN],joint_target[ID_EYES_LEFT_LR],joint_target[ID_EYES_BOTH_UD]);

        p.time_from_start = ros::Duration(1.2 * 1.0 / humotion::server::Server::MOTION_UPDATERATE);

        msg.points.push_back(p);

        target_publisher.publish(msg);

    }

}

//! prepare and enable a joint

//! NOTE: this should also prefill the min/max positions for this joint

//! \param the enum id of a joint

void MekaJointInterface::enable_joint(int e){

    //meka does not support this, joints are always enabled

}

//! shutdown and disable a joint

//! \param the enum id of a joint

void MekaJointInterface::disable_joint(int e){

    //meka does not support this, joints are always enabled

}

void MekaJointInterface::store_min_max(int id, float min, float max){

    joint_min[id] = min;

    joint_max[id] = max;

}

void MekaJointInterface::init_joints(){

    store_min_max(ID_NECK_TILT, -37, 1);

    store_min_max(ID_NECK_PAN, -70, 70);

    store_min_max(ID_NECK_ROLL, -1, 1);

    store_min_max(ID_EYES_BOTH_UD, -1, 1);

    store_min_max(ID_EYES_LEFT_LR, -1, 1);

    store_min_max(ID_EYES_RIGHT_LR, -1, 1);

    store_min_max(ID_EYES_LEFT_LID_UPPER, -1, 1);

    store_min_max(ID_EYES_LEFT_LID_LOWER, -1, 1);

    store_min_max(ID_EYES_RIGHT_LID_UPPER, -1, 1);

    store_min_max(ID_EYES_RIGHT_LID_LOWER, -1, 1);

    store_min_max(ID_EYES_LEFT_BROW, -1, 1);

    store_min_max(ID_EYES_RIGHT_BROW, -1, 1);

    store_min_max(ID_LIP_CENTER_UPPER, -1, 1);

    store_min_max(ID_LIP_CENTER_LOWER, -1, 1);

    store_min_max(ID_LIP_LEFT_UPPER, -1, 1);