(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Encoder

Description: Reading encoders and calculation of odom.

Tutorial Level: BEGINNER

Next Tutorial: IMU

Starting Encoders

Connect via SSH to the Evarobot.

> ssh pi@evarobotDSK
> sudo -s

Execute evarobot_odometry.launch file.

# evarobot
> roslaunch evarobot_odometry evarobot_odometry.launch

In order to sync ros masters,

# evarobot
> roslaunch master_discovery_fkie master_discovery.launch

# evarobot
> roslaunch master_sync_fkie master_sync.launch

Reading Odometry via Terminal

Execute synchronisation nodes, to reach evarobot ros master.

# pc
> roslaunch master_discovery_fkie master_discovery.launch

# pc
> roslaunch master_sync_fkie master_sync.launch

You can echo odom topic

# pc
> rostopic echo /odom

To get information about the topic

# pc
> rostopic info /odom

Writing a Simple Subscriber for Odometry

Use the catkin_create_pkg script to create a new package called 'evarobot_odom_subs' which depends on nav_msgs, roscpp, and rospy:

> cd ~/catkin_ws/src
> catkin_create_pkg evarobot_odom_subs nav_msgs rospy roscpp

Create a src directory in the evarobot_odom_subs package directory.

> mkdir -p ~/catkin_ws/src/evarobot_odom_subs/src

Create the src/odom_listener.cpp file within the evarobot_odom_subs package.

> cd ~/catkin_ws/src/evarobot_odom_subs/src
> gedit odom_listener.cpp

And paste the following inside odom_listener.cpp:

   1 #include "ros/ros.h"
   2 #include "nav_msgs/Odometry.h"
   3 
   4 /**
   5  * This tutorial demonstrates simple receipt of position and speed of the Evarobot over the ROS system.
   6  */
   7 
   8 /**
   9  * Callback function executes when new topic data comes.
  10  * Task of the callback function is to print data to screen.
  11  */
  12 void chatterCallback(const nav_msgs::Odometry::ConstPtr& msg)
  13 {
  14   ROS_INFO("Seq: [%d]", msg->header.seq);
  15   ROS_INFO("Position-> x: [%f], y: [%f], z: [%f]", msg->pose.pose.position.x,msg->pose.pose.position.y, msg->pose.pose.position.z);
  16   ROS_INFO("Orientation-> x: [%f], y: [%f], z: [%f], w: [%f]", msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
  17   ROS_INFO("Vel-> Linear: [%f], Angular: [%f]", msg->twist.twist.linear.x,msg->twist.twist.angular.z);
  18 }
  19 
  20 int main(int argc, char **argv)
  21 {
  22   /**
  23    * The ros::init() function needs to see argc and argv so that it can perform
  24    * any ROS arguments and name remapping that were provided at the command line.
  25    * For programmatic remappings you can use a different version of init() which takes
  26    * remappings directly, but for most command-line programs, passing argc and argv is
  27    * the easiest way to do it.  The third argument to init() is the name of the node.
  28    *
  29    * You must call one of the versions of ros::init() before using any other
  30    * part of the ROS system.
  31    */
  32   ros::init(argc, argv, "odom_listener");
  33 
  34   /**
  35    * NodeHandle is the main access point to communications with the ROS system.
  36    * The first NodeHandle constructed will fully initialize this node, and the last
  37    * NodeHandle destructed will close down the node.
  38    */
  39   ros::NodeHandle n;
  40 
  41   /**
  42    * The subscribe() call is how you tell ROS that you want to receive messages
  43    * on a given topic.  This invokes a call to the ROS
  44    * master node, which keeps a registry of who is publishing and who
  45    * is subscribing.  Messages are passed to a callback function, here
  46    * called chatterCallback.  subscribe() returns a Subscriber object that you
  47    * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
  48    * object go out of scope, this callback will automatically be unsubscribed from
  49    * this topic.
  50    *
  51    * The second parameter to the subscribe() function is the size of the message
  52    * queue.  If messages are arriving faster than they are being processed, this
  53    * is the number of messages that will be buffered up before beginning to throw
  54    * away the oldest ones.
  55    */
  56   ros::Subscriber sub = n.subscribe("odom", 1000, chatterCallback);
  57 
  58   /**
  59    * ros::spin() will enter a loop, pumping callbacks.  With this version, all
  60    * callbacks will be called from within this thread (the main one).  ros::spin()
  61    * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
  62    */
  63   ros::spin();
  64 
  65   return 0;
  66 }

> cd ..
> gedit CMakeLists.txt

The generated CMakeLists.txt should look like this

cmake_minimum_required(VERSION 2.8.3)
project(evarobot_odom_subs)

find_package(catkin REQUIRED COMPONENTS
  nav_msgs
  roscpp
  rospy
)

catkin_package()

include_directories(
  ${catkin_INCLUDE_DIRS}
)

add_executable(odom_listener src/odom_listener.cpp)
add_dependencies(odom_listener nav_msgs_generate_messages_cpp)
 target_link_libraries(odom_listener
   ${catkin_LIBRARIES}
 )

Now run catkin_make

> cd ~/catkin_ws/
> catkin_make

To run odom_listener,

> rosrun evarobot_odom_subs odom_listener

Wiki: Robots/evarobot/Tutorials/indigo/Encoder (last edited 2015-09-17 08:05:23 by makcakoca)