add /imu and /odom topic to HrpsysSeqROSBridge

[garaemon]

forked from start-jsk/rtmros_gazebo#24

[garaemon]

$ rosmsg show sensor_msgs/Imu
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Quaternion orientation
  float64 x
  float64 y
  float64 z
  float64 w
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
  float64 x
  float64 y
  float64 z
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
  float64 x
  float64 y
  float64 z
float64[9] linear_acceleration_covariance

$ rosmsg show nav_msgs/Odometry
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
  float64[36] covariance
geometry_msgs/TwistWithCovariance twist
  geometry_msgs/Twist twist
    geometry_msgs/Vector3 linear
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 angular
      float64 x
      float64 y
      float64 z
  float64[36] covariance

[garaemon]

@eisoku9618 @snozawa
How can i get the imu data from hrpsys?

[garaemon]

https://github.com/garaemon/rtmros_common/tree/support-odom-imu-topic
this is the branch i'm working on.

[garaemon]

I'd like to connect the data from imu sensor to HrpsysSeqStateBridge and publish the data to /imu.

[garaemon]

hrpsys.py

         # connection for kf
         if self.kf:
             #   currently use first acc and rate sensors for kf
             s_acc=filter(lambda s : s.type == 'Acceleration', self.sensors)
             if (len(s_acc)>0) and self.rh.port(s_acc[0].name) != None: # check existence of sensor ;; currently original HRP4C.xml has different naming      rule of gsensor and gyrometer
                 connectPorts(self.rh.port(s_acc[0].name), self.kf.port('acc'))
             s_rate=filter(lambda s : s.type == 'RateGyro', self.sensors)
             if (len(s_rate)>0) and self.rh.port(s_rate[0].name) != None: # check existence of sensor ;; currently original HRP4C.xml has different namin     g rule of gsensor and gyrometer
                 connectPorts(self.rh.port(s_rate[0].name), self.kf.port("rate"))
             connectPorts(self.seq.port("accRef"), self.kf.port("accRef"))

according the code above, connection is like this:

rh.?? =>  kf.acc
rh.?? => kf.rate
seq.accRef => kf.accRef

[snozawa]

Exactly.
I think IMU data is already connected to HrpsysSeqStateRosBridge,
e.g., kf.rtc:rpy is connected in hrpsys_ros_bridge.launch.

[garaemon]

If possible, I'd like to read rate and accRef.
kf.rtc:rpy is a result of kalman-filter inside of hrpsys, right?

And I'm wondering whether we should use kf.rtc:rpy as imu orientation in ROS or not...

[snozawa]

kf.rtc:rpy is a result of kalman-filter inside of hrpsys, right?

Yes.

[snozawa]

You just need to add sensor connection like this to hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py:

             s_acc=filter(lambda s : s.type == 'Acceleration', self.sensors)
             if (len(s_acc)>0) and self.rh.port(s_acc[0].name) != None: # check existence of sensor ;; currently original HRP4C.xml has different naming      rule of gsensor and gyrometer
                 connectPorts(self.rh.port(s_acc[0].name), self.kf.port('acc'))
             s_rate=filter(lambda s : s.type == 'RateGyro', self.sensors)
             if (len(s_rate)>0) and self.rh.port(s_rate[0].name) != None: # check existence of sensor ;; currently original HRP4C.xml has different namin     g rule of gsensor and gyrometer
                 connectPorts(self.rh.port(s_rate[0].name), self.kf.port("rate"))

Please write these connection in hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py instead of these connection in hrpsys_ros_bridge.launch
because the program to connect ports requires sensor names from VRML.

[garaemon]

gotcha, I will

2014年4月16日水曜日、snozawanotifications@github.comさんは書きました:

You just need to add sensor connection like this to
hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py:

         s_acc=filter(lambda s : s.type == 'Acceleration', self.sensors)
         if (len(s_acc)>0) and self.rh.port(s_acc[0].name) != None: # check existence of sensor ;; currently original HRP4C.xml has different naming      rule of gsensor and gyrometer
             connectPorts(self.rh.port(s_acc[0].name), self.kf.port('acc'))
         s_rate=filter(lambda s : s.type == 'RateGyro', self.sensors)
         if (len(s_rate)>0) and self.rh.port(s_rate[0].name) != None: # check existence of sensor ;; currently original HRP4C.xml has different namin     g rule of gsensor and gyrometer
             connectPorts(self.rh.port(s_rate[0].name), self.kf.port("rate"))

Please write these connection in
hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py instead of these
connection in hrpsys_ros_bridge.launch
because the program to connect ports requires sensor names from VRML.

—
Reply to this email directly or view it on GitHubhttps://github.com//issues/409#issuecomment-40503023
.

from iPhone

[garaemon]

$ rtcat -l localhost:15005/kf.rtc
kf.rtc  Active
  Category       example
  Description    kalman filter
  Instance name  kf
  Type name      KalmanFilter
  Vendor         AIST
  Version        315.1.7
  Parent         
  Type           Monolithic
 +Extra properties
-Execution Context 0
  State  Stopped
  Kind   Periodic
  Rate   1000000.0
  Owner  kf
-DataInPort: rate
  dataport.data_type          IDL:RTC/TimedAngularVelocity3D:1.0
  dataport.dataflow_type      push,pull
  dataport.interface_type     corba_cdr
  dataport.subscription_type  Any
  port.port_type              DataInPort
 +Connected to  /localhost:15005/HRP2JSKNT(Robot)0.rtc:gyrometer
-DataInPort: acc
  dataport.data_type          IDL:RTC/TimedAcceleration3D:1.0
  dataport.dataflow_type      push,pull
  dataport.interface_type     corba_cdr
  dataport.subscription_type  Any
  port.port_type              DataInPort
 +Connected to  /localhost:15005/HRP2JSKNT(Robot)0.rtc:gsensor
-DataInPort: accRef
  dataport.data_type          IDL:RTC/TimedAcceleration3D:1.0
  dataport.dataflow_type      push,pull
  dataport.interface_type     corba_cdr
  dataport.subscription_type  Any
  port.port_type              DataInPort
 +Connected to  /localhost:15005/seq.rtc:accRef
-DataInPort: rpyIn
  dataport.data_type          IDL:RTC/TimedAngularVelocity3D:1.0
  dataport.dataflow_type      push,pull
  dataport.interface_type     corba_cdr
  dataport.subscription_type  Any
  port.port_type              DataInPort
-DataOutPort: rpy
  dataport.data_type          IDL:RTC/TimedOrientation3D:1.0
  dataport.dataflow_type      push,pull
  dataport.interface_type     corba_cdr
  dataport.subscription_type  flush,new,periodic
  port.port_type              DataOutPort
 +Connected to  /localhost:15005/lynx.host_cxt/manager.mgr/st.rtc:rpy
 +Connected to  /localhost:15005/lynx.host_cxt/manager.mgr/rmfo.rtc:rpy
 +Connected to  /localhost:15005/log.rtc:kf_rpy
 +Connected to  /localhost:15005/HrpsysSeqStateROSBridge0.rtc:baseRpy
-DataOutPort: rpy_raw
  dataport.data_type          IDL:RTC/TimedOrientation3D:1.0
  dataport.dataflow_type      push,pull
  dataport.interface_type     corba_cdr
  dataport.subscription_type  flush,new,periodic
  port.port_type              DataOutPort
-CorbaPort: KalmanFilterService
  port.port_type  CorbaPort
  Interface:
    Instance name  service0
    Type name      KalmanFilterService
    Polarity       Provided
  (2 other connections)

[garaemon]

which are the imu data ports?

[garaemon]

new PR #430 supports imu