OpenRTM-asit ROSパッチ(ros port)を使ってルンバを操縦する

前回OpenRTM-aistからROSを使えるようにしました。
次はお決まりですがルンバを動かします。

今回はOpenRTM-aistの仮想ジョイスティックコンポーネントを利用してルンバを動かします。

構成としては

TkJoyStick(RTC) -> JoyRTM2velROS(今回作ったコンポーネント)(RTC/ROS) -> いつもの自作ルンバノード（ROS)

という感じです。
TkJoyStickはOpenRTM-aistのサンプルに入っていて、ジョイスティックっぽいGUIです。

面倒なのでビデオ解説です。



JoyRTM2velROSのソースはTkJoyStickからの入力(TimedFloatSeq)をROSのgeometry_msgs/Twist型に変換しているだけです。
やはりOpenRTMとROSは非常に似ていますね。

OpenRTM-aistのコードの自動生成は便利だけど修正がめんどくさいです。
とくにRosPortのように標準ツールでサポートされていないと大変です。
なにかいい方法があれば教えてください。

参考程度にcppのソースファイルだけ貼りつけておきます。
いつも通り適当です。

一応リポジトリに
otl-ros-pkg/otl_nav/JoyRTM2velROS
としておいておきました。

【参考サイト】
http://code.google.com/p/rtm-ros-robotics/wiki/rosport_Example
http://code.google.com/p/rtm-ros-robotics/wiki/RTM_HelloWorldSample



ヘッダファイル

// -*- C++ -*-
/*!
 * @file  JoyRTM2velROS.h
 * @brief convert Joystick sample port to ROS twist msg
 * @date  $Date$
 *
 * $Id$
 */

#ifndef JOYRTM2VELROS_H
#define JOYRTM2VELROS_H

#include <rtm/Manager.h>
#include <rtm/DataFlowComponentBase.h>
#include <rtm/CorbaPort.h>
#include <rtm/DataInPort.h>
#include <rtm/DataOutPort.h>
#include <rtm/idl/BasicDataTypeSkel.h>
#include <rtm/idl/ExtendedDataTypesSkel.h>
#include <rtm/idl/InterfaceDataTypesSkel.h>
#include <rtm/RosOutPort.h>
#include "geometry_msgs/Twist.h"

using namespace RTC;

/*!
 * @class JoyRTM2velROS
 * @brief convert Joystick sample port to ROS twist msg
 *
 */
class JoyRTM2velROS
  : public RTC::DataFlowComponentBase
{
 public:
  /*!
   * @brief constructor
   * @param manager Maneger Object
   */
  JoyRTM2velROS(RTC::Manager* manager);

  /*!
   * @brief destructor
   */
  ~JoyRTM2velROS();

  /***
   *
   * The initialize action (on CREATED->ALIVE transition)
   * formaer rtc_init_entry() 
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
   virtual RTC::ReturnCode_t onInitialize();

  /***
   *
   * The execution action that is invoked periodically
   * former rtc_active_do()
   *
   * @param ec_id target ExecutionContext Id
   *
   * @return RTC::ReturnCode_t
   * 
   * 
   */
   virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);

 protected:

  // Configuration variable declaration
  // 
  /*!
   * 
   * - Name:  max_linear_velocity
   * - DefaultValue: 0.1
   */
  double m_max_linear_velocity;
  /*!
   * 
   * - Name:  max_angular_velocity
   * - DefaultValue: 0.2
   */
  double m_max_angular_velocity;
  // 

  // DataInPort declaration
  // 
  TimedFloatSeq m_joyInput;
  /*!
   */
  InPort m_joyInputIn;
  
  // ROS Port
  geometry_msgs::Twist m_rosVel;
  RosOutPort m_rosVelOut;

 private:

};


extern "C"
{
  DLL_EXPORT void JoyRTM2velROSInit(RTC::Manager* manager);
};

#endif // JOYRTM2VELROS_H

実体(.cpp)

// -*- C++ -*-
/*!
 * @file  JoyRTM2velROS.cpp
 * @brief convert Joystick sample port to ROS twist msg
 * @date $Date$
 *
 * $Id$
 */

#include "JoyRTM2velROS.h"

// Module specification
// <rtc-template block="module_spec">
static const char* joyrtm2velros_spec[] =
  {
    "implementation_id", "JoyRTM2velROS",
    "type_name",         "JoyRTM2velROS",
    "description",       "convert Joystick sample port to ROS twist msg",
    "version",           "1.0.0",
    "vendor",            "OTL",
    "category",          "Sample",
    "activity_type",     "PERIODIC",
    "kind",              "DataFlowComponent",
    "max_instance",      "1",
    "language",          "C++",
    "lang_type",         "compile",
    // Configuration variables
    "conf.default.max_linear_velocity", "0.2",
    "conf.default.max_angular_velocity", "0.4",
    // Widget
    "conf.__widget__.max_linear_velocity", "slider",
    "conf.__widget__.max_angular_velocity", "slider",
    // Constraints
    ""
  };
// </rtc-template>

/*!
 * @brief constructor
 * @param manager Maneger Object
 */
JoyRTM2velROS::JoyRTM2velROS(RTC::Manager* manager)
    // <rtc-template block="initializer">
  : RTC::DataFlowComponentBase(manager),
    m_joyInputIn("joy_input", m_joyInput)

    // </rtc-template>
    ,
    m_rosVelOut("cmd_vel", "rtm2ros", m_rosVel)
{
}

/*!
 * @brief destructor
 */
JoyRTM2velROS::~JoyRTM2velROS()
{
}



RTC::ReturnCode_t JoyRTM2velROS::onInitialize()
{
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set InPort buffers
  addInPort("joy_input", m_joyInputIn);

  // Set OutPort buffer

  // Set service provider to Ports

  // Set service consumers to Ports

  // Set CORBA Service Ports

  // </rtc-template>

  // <rtc-template block="bind_config">
  // Bind variables and configuration variable
  bindParameter("max_linear_velocity", m_max_linear_velocity, "0.2");
  bindParameter("max_angular_velocity", m_max_angular_velocity, "0.4");
  // </rtc-template>

  addRosOutPort("ros_velocity", m_rosVelOut);

  return RTC::RTC_OK;
}


RTC::ReturnCode_t JoyRTM2velROS::onExecute(RTC::UniqueId ec_id)
{
  if(m_joyInputIn.isNew()) {
    m_joyInputIn.read();

    m_rosVel.linear.x = m_joyInput.data[1] * 0.005;
    m_rosVel.angular.z = -m_joyInput.data[0] * 0.010;

    if ( m_max_linear_velocity < m_rosVel.linear.x) {
      m_rosVel.linear.x = m_max_linear_velocity;
    } else if (-m_max_linear_velocity > m_rosVel.linear.x) {
      m_rosVel.linear.x = -m_max_linear_velocity;
    } else if (fabs(m_rosVel.linear.x) < 0.01) {
      m_rosVel.linear.x = 0.0;
    }
    if ( m_max_angular_velocity < m_rosVel.angular.z) {
      m_rosVel.angular.z = m_max_angular_velocity;
    } else if (-m_max_angular_velocity > m_rosVel.angular.z) {
      m_rosVel.angular.z = -m_max_angular_velocity;
    } else if (fabs(m_rosVel.angular.z) < 0.01) {
      m_rosVel.angular.z = 0.0;
    }

    m_rosVelOut.write();
  }
  return RTC::RTC_OK;
}



extern "C"
{

  void JoyRTM2velROSInit(RTC::Manager* manager)
  {
    coil::Properties profile(joyrtm2velros_spec);
    manager->registerFactory(profile,
                             RTC::Create<JoyRTM2velROS>,
                             RTC::Delete<JoyRTM2velROS>);
  }

};