ROS 进阶学习笔记(19)-- Add Extra Gyro Sensor for ROOMBA-Based Turtlebot

===Add Extra Gyro Sensor for ROOMBA-Based Turtlebot===

I write this in English for covering more friends globally. Please Indicate the SourceSonictl_blog_csdn.

== Background of this problem ==

As I'm following the book "ros by example_hydro_vol1" written by R. Patrick Goeble, In order to follow the instances in that book, I implemented a turtlebot-like turtlebot based on the iRobot Create2/ROOMBA_5xx/ROOMBA_6xx/etc. One of the big differences should
be that the original turtlebot is based on the "iRobot Create" which has a 25pin(img) connector and the driver for it "turtlebot_driver"(ROS
package) is written for that connector. ref
link. But, the iRobot Create2/ROOMBA_5xx/ROOMBA_6xx/etc has no 25pin connector.

I guess that 25pin-connector receives analog voltage values and send back to the Controller(Ubuntu-PC) through the 7pin Mini-DIN Connector via the RS232-USART serial protocol on TTL voltage. (refer:Calibrate
Odometry and Gyro - ROS wiki )

== Background knowledge about this problem ==

Before handling this, you should be a little familiar about ROS and ROS repositories.

You should have the items below:
  Learned all the tutorials for ROS beginners and know what is stack/package/catkin/topic/node/msg/launch/parameter/../etc.
  Known basic concepts/ideas about OOP/C++/python

== The structure of source code files about this problem ==

As the background I have described, I'm following the rbx(Ros by Example) package and steps. But the rbx packages/launch_files are developed based onthe turtlebot
stacks (which containsturtlebot_bringup<-turtlebot_node<-turtlebot_driver<-...) from ROS repositories. So, We will look into turtlebot stacks and find how does the gyro data processed/transferred by the packages
of:turtlebot_node,
turtlebot_bringup and so on. Please Indicate the Source
Sonictl_blog_csdn.

== The structure of the source files that process the imu_data ==

I describe this structure by the following:

  Start: User: launch rbx1_bringup by : $ roslaunch rbx1_bringup turtlebot_minimal_create.launch

  Next: ROS : file( tb_create_mobile_base.launch.xml ) is launching.

Let's look into this file:

There are two nodes about gyro_sensor lauched: turtlebot_node.py and robot_pose_ekf. And there are some parameters about gyro_sensor:"has_gyro"=true and"robot_type"=roomba
of turtlebot_node,"imu_used"=true and"output_frame"=odom of robot_pose_ekf. Actually, most of the parameters ofrobot_pose_ekf
are used for gyros.

 

The imu/gyro's data is firstly got by turtlebot_node.py, from the turtlebot_driver.py and theroomba_sensor_handler.py
, via the sensor_handler object. And the data is stored in thesensor_state object which belongs to theTurtlebotSensorState()
Class. We can see the source code ofturtlebot_node.py, and find out the relationships of them. Please Indicate the SourceSonictl_blog_csdn.

(ps: /opt/ros/hydro/lib/python2.7/dist-packages/create_node/ path, in your hydro-ROS installed ubuntu PC, stores the python source files mentioned above.)

(ps: /opt/ros/hydro/lib/create_node/turtlebot_node.py may meet your interests.)

What's next? When the sensor_state object got the sensor's data, it was assigned to thes object (See the "s = self.sensor_state" in the turtlebot_node.pysource
code.). There is another _gyro
object which takes "s" and called its publish() method. Let's see what is_gyro object. Please Indicate the SourceSonictl_blog_csdn.

== The _gyro object - who published the first topic of imu_data ==

In the turtlebot_node.py source code, we find that "self._gyro = TurtlebotGyro()" declares an object "self._gyro" which belongs toTurtlebotGyro() Class (defined by file: [/opt/ros/hydro/lib/python2.7/dist-packages/create_node/gyro.py]).
It's theTurtlebotGyro() Class has a method:publish() we mentioned above, if you look into thegyro.pysource
file.

You feel complex? Heh. It's it. That's why it takes me a full day to figure this out. Let's go on...

So, the publish() method of TurtlebotGyro() class takes the
sensor_state.user_analog_input value and assigned it to
imu_data.angular_velocity.z after some simple reshaping calculations which are about the parametergyro_measurement_range and mathematics.

Finally, the imu_data is published by the publisher(Type:sensor_msgs.msg.Imu Topic:imu/data & imu/raw) at line 81 and line 90 of the sourcefile(link:https://github.com/code-iai/turtlebot_roomba/blob/master/turtlebot_node/src/turtlebot_node/gyro.py#L81)

== The flow of the imu_data messages ==

Once the imu/data topic is published out, the nodes that subscribe to it will be discussed here.

In file we mentioned above: tb_create_mobile_base.launch.xml
    <remap from="imu/data" to="mobile_base/sensors/imu_data" />

    <remap from="imu/raw" to="mobile_base/sensors/imu_data_raw" />
So we check which node subscribes to "mobile_base/sensors/imu_data".

The first node subscribing to "mobile_base/sensors/imu_data" is [robot_pose_ekf], and [robot_pose_ekf] subscribes to /odom & /mobile_base/sensors/imu_data (tip: You can check this by command: $ rosnode -info /robot_pose_ekf), and publishes: /odom_combined

In the rbx book, Chapter 7.6.4-Timed Out and Back using a Real Robot, the author asks us to launch the commands below:
  $ roslaunch rbx1_bringup turtlebot_minimal_create.launch

   (start the nodes to drive and publish/process sensor data)

  $ roslaunch rbx1_bringup odom_ekf.launch

   (run the node "odom_ekf" which belongs to rbx1_bringup package, written by the author)

  $ rosrun rviz rviz -d `rospack find rbx1_nav`/nav_ekf.rviz

   (run the .rviz file configured rviz to monitor our /odom_ekf topic published by node "odom_ekf")

  $ rosrun rbx1_nav timed_out_and_back.py

   (make the turtlebot move so that we can see the effect better)

Finally we get the result of lovely orange arrows:

== The Steps of My Implementation of Adding Extra Gyro Sensor for ROOMBA ==

I'm sorry the most wanted part is finally shown at the last order now.


I'll show the steps concisely because I have described all the details above and have wasted you too much time.

Step1:

Get your gyro sensor hardware(chip? board? aduino?) connect to your Ubuntu-PC and make your sensor data can be published in ROS message on ROS topic('imu/angular_vel' for instance). The ROS message should be of type ofgeometry_msgs.msg.Twist().
Please Indicate the Source Sonictl_blog_csdn.

Step2:

Modify and add subscriber in your turtlebot_node.py source file:

for instance:

self.imu_sub=rospy.Subscriber("imu/angular_vel", Twist, self.callbackimu)

This makes your 'turtlebot' node receive your gyro data published by Step1. In your callbackimu method, take the  data on topic 'imu/angular_vel' and assign it to self.add_gyro_data.angular.z. The object self.add_gyro_data should be of type of geometry_msgs.msg.Twist()
obviously. The callbackimu method:

self.temp=Twist() #declare the Twist object for addGyroData when initialize

...
def callbackimu(self, data):
self.add_gyro_data.angular.z = data.angular.z

Step3:

Modify the gyro.py source file and make thepublish() method can have one more argument:

def publish(self, sensor_state, last_time, input_data)

       Absolutely, you should take input_data to do some calculations and assign it to self.imu_data.angular_velocity.z for publishing it out. So, slightly modify the publish() method to meet your needs. Please Indicate the SourceSonictl_blog_csdn.

Step4:

In turtlebot_node.py , Try to publish the self.add_gyro_data object out by calling theself._gyro.publish() method.

if self._gyro:
self._gyro.publish(s, last_time, self.add_gyro_data)

Check if you have achieved what you want, as the figure shown above. Please Indicate the SourceSonictl_blog_csdn.

Contact me

You can write to me your comment or ideas via:     sonictl    A-T    sina   D-O-T   com  (no spaces)