ROS x LeapMotionのお勉強

やりたいこと

ROSでLeap Motionを使ってみる

www.leapmotion.com

教科書

ROSプログラミング | 森北出版株式会社

ライセンス

  <maintainer email="fei@kanto-gakuin.ac.jp">Fei Qian</maintainer>
  <license>BSD</license>
  <author>Fei Qian</author>

準備

SDKのダウンロード V2 Tracking — Leap Motion Developer

64bitバージョン install

sudo dpkg --install Leap-2.3.1+31549-x64.deb

ubuntu16.04でもし動かなければ Ubuntu 16.04にLeapMotion導入 - Qiita を参考に修正

• leapを起動したい時

sudo service leapd start

• 止めたい時

sudo service leapd stop

• 状態の確認がしたい時

sudo service leapd status

• 設定

LeapControlPanel --showsettings

• Visualizer

Visualizer

パスの設定

export PYTHONPATH=$PYTHONPATH:$LEAP_SDK/lib:$LEAP_SDK/lib/x64
export LEAP_SDK=~/ros_ws/book/LeapDeveloperKit_2.3.1+31549_linux/LeapSDK

ROSで起動

cppの場合は自作する必要あり

以下を参考に動かせる

• leap_motion - ROS Wiki

• GitHub - ros-drivers/leap_motion: Leap Motion ROS integration

• GitHub - YuOhara/leap_motion at leap_camera

C++で実装

├── CMakeLists.txt 
└── leap_teleop
    ├── CMakeLists.txt
    ├── launch
    │   └── leap_turtlesim.launch
    ├── msg
    │   └── leap_motion.msg
    ├── package.xml
    └── src
        ├── leap_motion_publisher.cpp
        ├── leap_motion_subscriber.cpp
        ├── leap_turtle_teleop.cpp

msg

Header header
uint32 hand_id
geometry_msgs/Vector3 direction
geometry_msgs/Vector3 normal
geometry_msgs/Vector3 velocity
geometry_msgs/Vector3 palmpos
geometry_msgs/Vector3 ypr

実装配布

Listenerクラスを継承して、一部の必要なメソッドをオーバーライド

#include <Leap.h>
#include <ros/ros.h>
#include <leap_teleop/leap_motion.h>

using namespace Leap;

class HandsListener : public Listener {
    public:
    ros::NodeHandle nh;
    ros::Publisher  pub;

    /*
    * Called once, when this Listener object is newly added to a Controller.
    */
    virtual void onInit(const Controller&);

    /* 
    * Called when the Controller object connects to the Leap Motion software 
    * and the Leap Motion hardware device is plugged in, or when this Listener 
    * object is added to a Controller that is already connected.
    */
    virtual void onConnect(const Controller&);

    /*
    * Called when the Controller object disconnects from the Leap Motion software 
    * or the Leap Motion hardware is unplugged.
    */
    virtual void onDisconnect(const Controller&){ROS_DEBUG("Disconnected");};

    /*
    * Called when this Listener object is removed from the Controller or the 
    * Controller instance is destroyed.
    */
    virtual void onExit(const Controller&){ROS_DEBUG("Exited");};

    /*
    * Called when a new frame of hand and finger tracking data is available.
    */
    virtual void onFrame(const Controller&);

    /*
    * Called when this application becomes the foreground application.
    */
    virtual void onFocusGained(const Controller&) {ROS_DEBUG("Focus Gained");};

    /*
    * Called when this application loses the foreground focus.
    */
    virtual void onFocusLost(const Controller&){ROS_DEBUG("Focus Lost");};

    /*
    * Called when a Leap Motion controller plugged in, unplugged, or the device changes state.
    */
    virtual void onDeviceChange(const Controller&){ROS_DEBUG("Device Changed");};

    /*
    * Called when the Leap Motion daemon/service connects to your application Controller.
    */
    virtual void onServiceConnect(const Controller&){ROS_DEBUG("Service Connected");};

    /*
    * Called if the Leap Motion daemon/service disconnects from your application Controller.
    */
    virtual void onServiceDisconnect(const Controller&){ROS_DEBUG("Service Disconnected");};
    private:
};

void HandsListener::onInit(const Controller& controller) 
{

    std::cout << "Initialized" <<std::endl;
    pub = nh.advertise<leap_teleop::leap_motion>("hands_motion", 1);
}

void HandsListener::onConnect(const Controller& controller) 
{
    std::cout << "Connected" << std::endl;
    controller.enableGesture(Gesture::TYPE_CIRCLE);
    controller.enableGesture(Gesture::TYPE_KEY_TAP);
    controller.enableGesture(Gesture::TYPE_SCREEN_TAP);
    controller.enableGesture(Gesture::TYPE_SWIPE);
}

void HandsListener::onFrame(const Controller& controller) 
{
    // Get the most recent frame and report some basic information
    const Frame frame = controller.frame();
    leap_teleop::leap_motion msg;

    msg.header.frame_id = "leap_motion_pub";
    msg.header.stamp = ros::Time::now();

    HandList hands = frame.hands();
    const Hand hand = hands[0];

    Vector normal = hand.palmNormal();
    Vector direction = hand.direction();
    Vector velocity = hand.palmVelocity();
    Vector position = hand.palmPosition();

    msg.hand_id = hand.id();

    msg.direction.x = direction[0];
    msg.direction.y = direction[1];
    msg.direction.z = direction[2];

    msg.normal.x = normal[0];
    msg.normal.y = normal[1];
    msg.normal.z = normal[2];

    msg.velocity.x = velocity[0];
    msg.velocity.y = velocity[1];
    msg.velocity.z = velocity[2];

    msg.palmpos.x = position[0];
    msg.palmpos.y = position[1];
    msg.palmpos.z = position[2];

    msg.ypr.x = direction.pitch() * RAD_TO_DEG ;
    msg.ypr.y = normal.roll() * RAD_TO_DEG;
    msg.ypr.z = direction.yaw() * RAD_TO_DEG;
    
    std::cout << "      Hand ID:" << hand.id() << std::endl;
    std::cout << " PalmPosition:" << hand.palmPosition() << std::endl;
    std::cout << " PalmVelocity:" << hand.palmVelocity() << std::endl;
    std::cout << "   PalmNormal:" << hand.palmNormal() << std::endl;
    std::cout << "PalmDirection:" << hand.direction() << std::endl;
    std::cout << " pitch:" << msg.ypr.x << std::endl;
    std::cout << "  roll:" << msg.ypr.y << std::endl;
    std::cout << "   yaw:" << msg.ypr.z << std::endl;
    std::cout << "--------------" << std::endl;

    pub.publish(msg);
}

int main(int argc, char** argv) 
{
    ros::init(argc, argv, "leap_motion_publisher");

    // Create a sample listener and controller
    HandsListener listener;
    Controller controller;
  
    // Have the sample listener receive events from the controller
    controller.addListener(listener);
    controller.setPolicyFlags(static_cast<Leap::Controller::PolicyFlag> (Leap::Controller::POLICY_IMAGES));

    ros::spin();

    // Remove the sample listener when done
    controller.removeListener(listener);
    return 0;
}

• onInitメソッドは、addListenerでリスナーを追加したときに呼び出される。

• onFrameはフレームデータが更新されたときに呼び出されるコールバックで、メッセージにカプセル化し、配布。

• PolicyFlageなどは Controller — Leap Motion C++ SDK v3.2 documentation 参照

実装購読

//#include <Leap.h>
#include <ros/ros.h>
#include <leap_teleop/leap_motion.h>

void callback(const leap_teleop::leap_motion::ConstPtr& msg)
{
    std::cout << "frame id: " << msg->header.frame_id << std::endl;
    std::cout << "tiemstamp: " << msg->header.stamp << std::endl;
    std::cout << "seq: " << msg->header.seq << std::endl;
    std::cout << "hand id: " << msg->hand_id << std::endl;
    std::cout << "direction: \n" << msg->direction << std::endl;
    std::cout << "normal: \n" << msg->normal << std::endl;
    std::cout << "velocity: \n" << msg->velocity << std::endl;
    std::cout << "palmpos: \n" << msg->palmpos << std::endl;
    std::cout << "ypr: \n" << msg->ypr << std::endl;
    std::cout << "--------------" << std::endl;
}


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

    ros::Subscriber sub = nh.subscribe<leap_teleop::leap_motion>(
        "/hands_motion", 10, callback);

    ros::spin();
    return 0;
}

CMakeList

cmake_minimum_required(VERSION 2.8.3)
project(leap_teleop)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
  tf
  cv_bridge
  image_transport
)

add_message_files(
   FILES
   leap_motion.msg
)

generate_messages(DEPENDENCIES std_msgs geometry_msgs sensor_msgs)

catkin_package(
   CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
)

include_directories(include 
  ${catkin_INCLUDE_DIRS}
  if($ENV{LEAP_SDK} $ENV{LEAP_SDK}/include)
)

if(DEFINED ENV{LEAP_SDK})
  add_executable(leap_pub src/leap_motion_publisher.cpp)
  target_link_libraries(leap_pub ${catkin_LIBRARIES} 
    ${Boost_LIBRARIES}
    ${catkin_LIBRARIES} $ENV{LEAP_SDK}/lib/x64/libLeap.so
  )
  add_dependencies(leap_pub leap_teleop_gencpp) # ビルド時の順番の指定!?

  add_executable(leap_sub src/leap_motion_subscriber.cpp)
  target_link_libraries(leap_sub ${catkin_LIBRARIES} 
    ${Boost_LIBRARIES}
    ${catkin_LIBRARIES} $ENV{LEAP_SDK}/lib/x64/libLeap.so
  )
  add_dependencies(leap_sub leap_teleop_gencpp)


  add_executable(leap_turtle_teleop src/leap_turtle_teleop.cpp)
  target_link_libraries(leap_turtle_teleop ${catkin_LIBRARIES} 
    ${Boost_LIBRARIES}
    ${catkin_LIBRARIES} $ENV{LEAP_SDK}/lib/x64/libLeap.so
  )
  add_dependencies(leap_turtle_teleop leap_teleop_gencpp)


  install(TARGETS leap_pub leap_sub leap_turtle_teleop
    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
  )

endif()

package.xml

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>camera_calibration_parsers</build_depend>
  <build_depend>camera_info_manager</build_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_depend>image_transport</build_depend>
  <build_depend>message_generation</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>roslib</build_depend>
  <build_depend>rospack</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>visualization_msgs</build_depend>

  <run_depend>camera_calibration_parsers</run_depend>
  <run_depend>camera_info_manager</run_depend>
  <run_depend>geometry_msgs</run_depend>
  <run_depend>image_transport</run_depend>
  <run_depend>message_runtime</run_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>roslib</run_depend>
  <run_depend>rospack</run_depend>
  <run_depend>rospy</run_depend>
  <run_depend>std_msgs</run_depend>
  <run_depend>visualization_msgs</run_depend>

実装亀制御

<launch>
    <!-- Turtlesim Node -->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

    <!-- leapmotion node -->
    <node pkg="leap_teleop" type="leap_pub" name="leap_motion" />

    <node pkg="leap_teleop" type="leap_turtle_teleop" name="leap_teleop" 
    launch-prefix="xterm -font r14 -bg darkblue -geometry 113x30+503+80 -e"
    required="true" 
    />    
</launch>

//#include <Leap.h>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h>
#include <leap_teleop/leap_motion.h>

class LeapTurtle
{
    public: 
    LeapTurtle();

    private:
    void leapCallback(const leap_teleop::leap_motion::ConstPtr& msg);
    ros::NodeHandle nh;
    ros::Publisher  pub;
    ros::Subscriber sub;
};

LeapTurtle::LeapTurtle()
{
    sub = nh.subscribe<leap_teleop::leap_motion>(
        "/hands_motion", 10, &LeapTurtle::leapCallback, this);
    pub = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1);
}

void LeapTurtle::leapCallback(const leap_teleop::leap_motion::ConstPtr& msg)
{
    std::cout << "frame id: " << msg->header.frame_id << std::endl;
    std::cout << "tiemstamp: " << msg->header.stamp << std::endl;
    std::cout << "seq: " << msg->header.seq << std::endl;
    std::cout << "hand id: " << msg->hand_id << std::endl;
    std::cout << "direction: \n" << msg->direction << std::endl;
    std::cout << "normal: \n" << msg->normal << std::endl;
    std::cout << "velocity: \n" << msg->velocity << std::endl;
    std::cout << "palmpos: \n" << msg->palmpos << std::endl;
    std::cout << "ypr: \n" << msg->ypr << std::endl;
    std::cout << "--------------" << std::endl;

    geometry_msgs::Twist twist;
    twist.linear.x  = -0.15*msg->ypr.x;
    twist.angular.z = 0.15*msg->ypr.y;
    ROS_INFO_STREAM("(" << msg->ypr.x << " " << msg->ypr.y << ")");

    pub.publish(twist);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "leap_turtle_teleop");
    LeapTurtle leap_turtle;

    ros::spin();
    return 0;
}

ステレオカメラとして動かしたい！

しかし、下記エラーで動かず。。。原因調査中。

[ERROR] [1510999568.734786707]: Skipping XML Document "/opt/ros/kinetic/share/gmapping/nodelet_plugins.xml" which had no Root Element.  This likely means the XML is malformed or missing.

• Skipping XML documentation · Issue #55 · ros-perception/slam_gmapping · GitHub

このissueが原因

• https://github.com/gavanderhoorn/slam_gmapping/tree/install_nodelet_plugin_xml

のソースからオーバーレイで解決

• GitHub - qboticslabs/leap_client: A ROS C++ wrapper around the Leap Motion API

leap motion公式HPの説明↓

• https://developer.leapmotion.com/documentation/v2/python/devguide/Leap_Images.html?proglang=python

• https://community.leapmotion.com/t/sdk-2-1-raw-data-get-pixel-position-xyz/1604/12

• Leap Motion SDK - YouTube