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



ヘッダファイル

view plainprint?

1. // -*- C++ -*-  
2. /*! 
3.  * @file  JoyRTM2velROS.h 
4.  * @brief convert Joystick sample port to ROS twist msg 
5.  * @date  $Date$ 
6.  * 
7.  * $Id$ 
8.  */  
9.   
10. #ifndef JOYRTM2VELROS_H  
11. #define JOYRTM2VELROS_H  
12.   
13. #include <rtm/Manager.h>  
14. #include <rtm/DataFlowComponentBase.h>  
15. #include <rtm/CorbaPort.h>  
16. #include <rtm/DataInPort.h>  
17. #include <rtm/DataOutPort.h>  
18. #include <rtm/idl/BasicDataTypeSkel.h>  
19. #include <rtm/idl/ExtendedDataTypesSkel.h>  
20. #include <rtm/idl/InterfaceDataTypesSkel.h>  
21. #include <rtm/RosOutPort.h>  
22. #include "geometry_msgs/Twist.h"  
23.   
24. using namespace RTC;  
25.   
26. /*! 
27.  * @class JoyRTM2velROS 
28.  * @brief convert Joystick sample port to ROS twist msg 
29.  * 
30.  */  
31. class JoyRTM2velROS  
32.   : public RTC::DataFlowComponentBase  
33. {  
34.  public:  
35.   /*! 
36.    * @brief constructor 
37.    * @param manager Maneger Object 
38.    */  
39.   JoyRTM2velROS(RTC::Manager* manager);  
40.   
41.   /*! 
42.    * @brief destructor 
43.    */  
44.   ~JoyRTM2velROS();  
45.   
46.   /*** 
47.    * 
48.    * The initialize action (on CREATED->ALIVE transition) 
49.    * formaer rtc_init_entry()  
50.    * 
51.    * @return RTC::ReturnCode_t 
52.    *  
53.    *  
54.    */  
55.    virtual RTC::ReturnCode_t onInitialize();  
56.   
57.   /*** 
58.    * 
59.    * The execution action that is invoked periodically 
60.    * former rtc_active_do() 
61.    * 
62.    * @param ec_id target ExecutionContext Id 
63.    * 
64.    * @return RTC::ReturnCode_t 
65.    *  
66.    *  
67.    */  
68.    virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);  
69.   
70.  protected:  
71.   
72.   // Configuration variable declaration  
73.   // <rtc-template block="config_declare">  
74.   /*! 
75.    *  
76.    * - Name:  max_linear_velocity 
77.    * - DefaultValue: 0.1 
78.    */  
79.   double m_max_linear_velocity;  
80.   /*! 
81.    *  
82.    * - Name:  max_angular_velocity 
83.    * - DefaultValue: 0.2 
84.    */  
85.   double m_max_angular_velocity;  
86.   // </rtc-template>  
87.   
88.   // DataInPort declaration  
89.   // <rtc-template block="inport_declare">  
90.   TimedFloatSeq m_joyInput;  
91.   /*! 
92.    */  
93.   InPort<timedfloatseq> m_joyInputIn;  
94.     
95.   // ROS Port  
96.   geometry_msgs::Twist m_rosVel;  
97.   RosOutPort<geometry_msgs::twist> m_rosVelOut;  
98.   
99.  private:  
100.   
101. };  
102.   
103.   
104. extern "C"  
105. {  
106.   DLL_EXPORT void JoyRTM2velROSInit(RTC::Manager* manager);  
107. };  
108.   
109. #endif // JOYRTM2VELROS_H  
110. </geometry_msgs::twist></timedfloatseq></rtc-template>  

// -*- 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)

view plainprint?

1. // -*- C++ -*-  
2. /*! 
3.  * @file  JoyRTM2velROS.cpp 
4.  * @brief convert Joystick sample port to ROS twist msg 
5.  * @date $Date$ 
6.  * 
7.  * $Id$ 
8.  */  
9.   
10. #include "JoyRTM2velROS.h"  
11.   
12. // Module specification  
13. // <rtc-template block="module_spec">  
14. static const char* joyrtm2velros_spec[] =  
15.   {  
16.     "implementation_id", "JoyRTM2velROS",  
17.     "type_name",         "JoyRTM2velROS",  
18.     "description",       "convert Joystick sample port to ROS twist msg",  
19.     "version",           "1.0.0",  
20.     "vendor",            "OTL",  
21.     "category",          "Sample",  
22.     "activity_type",     "PERIODIC",  
23.     "kind",              "DataFlowComponent",  
24.     "max_instance",      "1",  
25.     "language",          "C++",  
26.     "lang_type",         "compile",  
27.     // Configuration variables  
28.     "conf.default.max_linear_velocity", "0.2",  
29.     "conf.default.max_angular_velocity", "0.4",  
30.     // Widget  
31.     "conf.__widget__.max_linear_velocity", "slider",  
32.     "conf.__widget__.max_angular_velocity", "slider",  
33.     // Constraints  
34.     ""  
35.   };  
36. // </rtc-template>  
37.   
38. /*! 
39.  * @brief constructor 
40.  * @param manager Maneger Object 
41.  */  
42. JoyRTM2velROS::JoyRTM2velROS(RTC::Manager* manager)  
43.     // <rtc-template block="initializer">  
44.   : RTC::DataFlowComponentBase(manager),  
45.     m_joyInputIn("joy_input", m_joyInput)  
46.   
47.     // </rtc-template>  
48.     ,  
49.     m_rosVelOut("cmd_vel", "rtm2ros", m_rosVel)  
50. {  
51. }  
52.   
53. /*! 
54.  * @brief destructor 
55.  */  
56. JoyRTM2velROS::~JoyRTM2velROS()  
57. {  
58. }  
59.   
60.   
61.   
62. RTC::ReturnCode_t JoyRTM2velROS::onInitialize()  
63. {  
64.   // Registration: InPort/OutPort/Service  
65.   // <rtc-template block="registration">  
66.   // Set InPort buffers  
67.   addInPort("joy_input", m_joyInputIn);  
68.   
69.   // Set OutPort buffer  
70.   
71.   // Set service provider to Ports  
72.   
73.   // Set service consumers to Ports  
74.   
75.   // Set CORBA Service Ports  
76.   
77.   // </rtc-template>  
78.   
79.   // <rtc-template block="bind_config">  
80.   // Bind variables and configuration variable  
81.   bindParameter("max_linear_velocity", m_max_linear_velocity, "0.2");  
82.   bindParameter("max_angular_velocity", m_max_angular_velocity, "0.4");  
83.   // </rtc-template>  
84.   
85.   addRosOutPort("ros_velocity", m_rosVelOut);  
86.   
87.   return RTC::RTC_OK;  
88. }  
89.   
90.   
91. RTC::ReturnCode_t JoyRTM2velROS::onExecute(RTC::UniqueId ec_id)  
92. {  
93.   if(m_joyInputIn.isNew()) {  
94.     m_joyInputIn.read();  
95.   
96.     m_rosVel.linear.x = m_joyInput.data[1] * 0.005;  
97.     m_rosVel.angular.z = -m_joyInput.data[0] * 0.010;  
98.   
99.     if ( m_max_linear_velocity < m_rosVel.linear.x) {  
100.       m_rosVel.linear.x = m_max_linear_velocity;  
101.     } else if (-m_max_linear_velocity > m_rosVel.linear.x) {  
102.       m_rosVel.linear.x = -m_max_linear_velocity;  
103.     } else if (fabs(m_rosVel.linear.x) < 0.01) {  
104.       m_rosVel.linear.x = 0.0;  
105.     }  
106.     if ( m_max_angular_velocity < m_rosVel.angular.z) {  
107.       m_rosVel.angular.z = m_max_angular_velocity;  
108.     } else if (-m_max_angular_velocity > m_rosVel.angular.z) {  
109.       m_rosVel.angular.z = -m_max_angular_velocity;  
110.     } else if (fabs(m_rosVel.angular.z) < 0.01) {  
111.       m_rosVel.angular.z = 0.0;  
112.     }  
113.   
114.     m_rosVelOut.write();  
115.   }  
116.   return RTC::RTC_OK;  
117. }  
118.   
119.   
120.   
121. extern "C"  
122. {  
123.   
124.   void JoyRTM2velROSInit(RTC::Manager* manager)  
125.   {  
126.     coil::Properties profile(joyrtm2velros_spec);  
127.     manager->registerFactory(profile,  
128.                              RTC::Create<JoyRTM2velROS>,  
129.                              RTC::Delete<JoyRTM2velROS>);  
130.   }  
131.   
132. };  

// -*- 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>);
  }

};