Lab 2.3: Setup GPS and Inertial Measurement Unit (IMU) modules

Overview

The goal of this section is to read the values from both GPS module and IMU module installed on the wifibot.

GPS protocol

Before we get the data from GPS module, we have to understand the protocol it sends. The standard protocol that all civilian GPS modules send is called NMEA 0183. Basically different sentences contain different information (ex: number of satellite connections, speed, course, speed, longitude and latitude etc…). If we want to parse those sentences by ourselves, it will take times, therefore, we can use the package that has been design for parsing the NMEA protocol and extracting the information we want.

GPS Package for ROS

First we have to install the package, open terminal using following command:

sudo apt-get install ros-indigo-nmea-navsat-driver

Plugin GPS module and check the serial port of the module it uses. By default it is ttyUSB0 for BU-353S4 module.

Now use the NMEA parsing package using following command, make sure you are running roscore:

rosrun nmea_navsat_driver nmea_serial_driver _port:=/dev/ttyUSB0

Open new terminal to see what data we got via parsing node:

rostopic echo /fix

Then you will see something like below:

header:
  seq: 3
  stamp:
    secs: 1439806142
    nsecs: 222456932
  frame_id: /gps
status:
  status: -1
  service: 1
latitude: nan
longitude: nan
altitude: nan
position_covariance: [nan, 0.0, 0.0, 0.0, nan, 0.0, 0.0, 0.0, nan]
position_covariance_type: 1

To see the latitude, longitude or other values, you can move your wifibot outdoor. If the value still remain unchanged for 3 mins outdoor, unplug the GPS module and plug in again.

IMU Protocol and Query its Data

Unfortunately we do not have extant package for our IMU model  (YEI-TSS 3-Space Sensor). However, we could build one by ourself based on its data sheet.

According to data sheet, we know the module transmit the data via a serial port. Therefore, we can now use our previous script to communicate via certain serial port. Lets write a ROS node that continuously queries the data from IMU module.

Open the terminal and create a new .cpp script:

roscd my_ex1/src
gedit IMUQuery.cpp 

Paste on the following script:

 


#include "ros/ros.h"
#include "std_msgs/String.h"
#include <stdio.h>
#include <termios.h> //header contains the definitions used by the terminal I/O interfaces
#include <unistd.h> //read() write() close()
#include <fcntl.h>
#include <string.h>
#include <stdlib.h>
#include <iostream>
#include <sys/stat.h>
#include <stdint.h>
#define BAUDRATE B115200
#define SERIAL_PATH "/dev/ttyACM0"
int serial_fd;
int serial_read_ret, serial_write_ret;
struct termios serial_settings;
char serial_buffer_send[3];
char serial_buffer_recv[50] = {0};
int main(int argc, char **argv)
{
printf("Opening %s in Read/Write mode at 115200 8-N-1...",SERIAL_PATH);
fflush(stdout);
//Try opening serial port
serial_fd = open(SERIAL_PATH,O_RDWR|O_NOCTTY);
if(serial_fd == -1) { //Checks the availability of the Serial Port
printf("Failed.\n");
fflush(stdout);
return 0;
} else {
printf("Success.\n");
fflush(stdout);
tcgetattr(serial_fd, &serial_settings); //Get Current Settings of the Port
cfsetispeed(&serial_settings,BAUDRATE); //Set Input Baudrate
cfsetospeed(&serial_settings,BAUDRATE); //Set Output Baudrate
serial_settings.c_cflag &= ~PARENB; //Mask Parity Bit as No Parity
serial_settings.c_cflag &= ~CSTOPB; //Set Stop Bits as 1 or else it will be 2
serial_settings.c_cflag &= ~CSIZE; //Clear the current no. of data bit setting
serial_settings.c_cflag |= CS8; //Set no. of data bits as 8 Bits
}
ros::init(argc, argv, "IMUQuery");
ros::NodeHandle n;
ros::Publisher IMUQuery_pub = n.advertise<std_msgs::String>("IMUData", 10);
ros:: Rate loop_rate(1);
std_msgs::String msg;
while (ros::ok())
{
serial_buffer_send[0]=(char)58;
serial_buffer_send[1]=(char)49;
serial_buffer_send[2]=(char)10;
serial_write_ret = write(serial_fd,serial_buffer_send,sizeof(serial_buffer_send));
serial_read_ret=read(serial_fd,serial_buffer_recv,sizeof(serial_buffer_recv));
std::stringstream ss;
ss << serial_buffer_recv;
msg.data = ss.str();
IMUQuery_pub.publish(msg);
//ROS_INFO("%s\n", msg.data.c_str());
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

Compile the package, and do not forget to add following line to CMakeLists.txt:

rosbuild_add_executable(IMUQuery src/IMUQuery.cpp)

We use serial port of ttyACM0 and 115200 baud rate to communicate with IMU module. Here we query the Euler angle which is by default oriented with magnetic filed with north. You can query another data from it based on data sheet or even send command to re-orientate the module.

The topic we publish is called IMUData which is shown in green text in the script, and the message type we use is standard message. Later on we are going to create a node that simultaneously subscribes both GPS data and IMU data.

We can test the node

The Transform Library

Our vehicle has many sensors attached to it, so it will not be useful to operate these sensors without integrating  their data into the robot. The tf library helps us in keeping track of the robot components over time, and their orientation to the robot main body.

tf can operate in a distributed system. This means all the information about the coordinate frames of a robot is available to all ROS components on any computer in the system. There is no central server of transform information.

 

Broadcasting a Transform

we need to create the node that will do the work of broadcasting the laser → base_link, camera_depth_frame → base_link transform over ROS. Create the src/tf_broadcaster.cpp file in the a package of your choice.

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv){
  ros::init(argc, argv, “robot_tf_publisher”);
  ros::NodeHandle n;
  ros::Rate r(100);
  tf::TransformBroadcaster broadcaster;
  while(n.ok()){
    broadcaster.sendTransform(
     tf::StampedTransform(
        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
        ros::Time::now(),”base_link”, “laser”));
 
    broadcaster.sendTransform(
     tf::StampedTransform(
        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.3)),
        ros::Time::now(),”base_link”, “camera_depth_frame”));
    r.sleep();
  }
}
 

Do not forget to add to the file CMakefile.txt the line

rosbuild_add_executable(tf_broadcaster src/tf_broadcaster.cpp)

 

and to the file manifest.xml the below line

 <depend package=”tf”/>

 

Code Explanation

#include <tf/transform_broadcaster.h>

 

The tf package provides an implementation of a tf::TransformBroadcaster to help make the task of publishing transforms easier. To use the TransformBroadcaster, we need to include the tf/transform_broadcaster.h header file.

tf::TransformBroadcaster broadcaster;

 

Here, we create a TransformBroadcaster object that we’ll use later to send the base_link → laser, camera_depth_frame → base_link transform over the wire.

    broadcaster.sendTransform(
     tf::StampedTransform(
        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
        ros::Time::now(),”base_link”, “laser”));
 

This is where the real work is done. Sending a transform with a TransformBroadcaster requires five arguments. First, we pass in the rotation transform, which is specified by a btQuaternion for any rotation that needs to occur between the two coordinate frames. In this case, we want to apply no rotation, so we send in a btQuaternion constructed from pitch, roll, and yaw values equal to zero. Second, a btVector3 for any translation that we’d like to apply. We do, however, want to apply a translation, so we create a btVector3 corresponding to the laser’s x offset of 10cm and z offset of 20cm from the robot base. Third, we need to give the transform being published a timestamp, we’ll just stamp it with ros::Time::now(). Fourth, we need to pass the name of the parent node of the link we’re creating, in this case “base_link.” Fifth, we need to pass the name of the child node of the link we’re creating, in this case “laser.”

 

after starting the kinect and hokuyo laser run this tf_broadcaster, and this is how you create a relation among the nodes of the same robot.

Visualize multiple sensor feeds

Now we can view the data provided by the kinect and Hokuyo node on the base link.

on your machine start rviz

set the fixed frame on base_link

add the pointCloud2 and laserscan, you should be able to find the topics. After setting the correct topics you should be able to view the published data by both sensors.