Now, we are going to create a node that gets data from joy_node and published topics to control a robot model.

Now, it is necessary to create a robot 3D model like we have seen in the previous chapters and a node with the odometry. In the repository, you can check the urdf model in the chapter8_tutorials/urdf folder and you can check the code for odometry in the /src folder where the file is named c8_odom.cpp.

In this code, odometry is calculated using an initial position 0,0,0 and using velocity information from a geometry_msgs::Twist. The velocity of each wheel is calculated, and the position is updated using differential drive kinematics. You can run the odometry node after a roscore command typing the following in a terminal:

    $ rosrun chapter8_tutorials c8_odom  

To understand the topics used in this node, we can display the topic list; use the following command line:

    $ rosnode info /odom  

You will then see the following output, where /cmd_vel is the topic published by the node:

We can check the topic type of /cmd_vel. Use the following command line to know it:

    $ rostopic type /cmd_vel  

You will see this output:

    geometry_msgs/Twist  

To know the contents of this message, execute the following command line:

    $ rosmsg show geometry_msgs/Twist  

You will then see the two fields that are used to send the velocity:

OK, now that we have localized the topic and the structure to use, it is time to create a program to generate velocity commands using data from the joystick.

Create a new file, c8_teleop_joy.cpp, in the chapter8_tutorials/src directory and type in the following code snippet:

#include<ros/ros.h> 
#include<geometry_msgs/Twist.h> 
#include<sensor_msgs/Joy.h> 
#include<iostream> 
 
using namespace std; 
float max_linear_vel = 0.2; 
float max_angular_vel = 1.5707; 
 
class TeleopJoy{ 
  public: 
  TeleopJoy(); 
  private: 
  void callBack(const sensor_msgs::Joy::ConstPtr& joy); 
  ros::NodeHandle n; 
  ros::Publisher pub; 
  ros::Subscriber sub; 
  int i_velLinear , i_velAngular; 
}; 
 
TeleopJoy::TeleopJoy() 
{ i_velLinear = 1; 
  i_velAngular = 0; 
  n.param("axis_linear",i_velLinear,i_velLinear); 
  n.param("axis_angular",i_velAngular,i_velAngular); 
  pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",1); 
  sub = n.subscribe<sensor_msgs::Joy>("joy", 10, 
   &TeleopJoy::callBack, this); 
} 
 
void TeleopJoy::callBack(const sensor_msgs::Joy::ConstPtr& joy) 
{ 
  geometry_msgs::Twist vel; 
  vel.angular.z = max_angular_vel*joy->axes[0]; 
  vel.linear.x = max_linear_vel*joy->axes[1]; 
  pub.publish(vel); 
} 
 
int main(int argc, char** argv) 
{ 
  ros::init(argc, argv, "c8_teleop_joy"); 
  TeleopJoy teleop_joy; 
  ros::spin(); 
} 

Now, we are going to break the code to explain how it works. In the main function, we create an instance of the TeleopJoy class:

int main(int argc, char** argv) 
{ 
 
  TeleopJoy teleop_joy; 
} 

In the constructor, four variables are initialized. The first two variables are filled using data from Parameter Server. These variables are joystick axes. The next two variables are the advertiser and the subscriber. The advertiser will publish a topic with the geometry_msgs::Twist type. The subscriber will get data from the topic with the name Joy. The node that is handling the joystick sends this topic:

TeleopJoy::TeleopJoy() 
{ i_velLinear = 1; 
  i_velAngular = 0; 
  n.param("axis_linear",i_velLinear,i_velLinear); 
  n.param("axis_angular",i_velAngular,i_velAngular); 
  pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",1); 
  sub = n.subscribe<sensor_msgs::Joy>("joy", 10, 
   &TeleopJoy::callBack, this); 
} 

Each time the node receives a message, the callBack() function is called. We create a new variable with the name vel, which will be used to publish data. The values of the axes of the joystick are assigned to the vel variable using coefficients to assign the maximum velocities. In this part, you can create a process with the data received before publishing it:

void TeleopJoy::callBack(const sensor_msgs::Joy::ConstPtr& joy) 
{ 
  geometry_msgs::Twist vel; 
  vel.angular.z = max_angular_vel*joy->axes[0]; 
  vel.linear.x = max_linear_vel*joy->axes[1]; 
  pub.publish(vel); 
} 

Finally, the topic is published using pub.publish(vel). We are going to create a launch file for this example. In the launch file, we declare data for Parameter Server and launch the joy and c8_teleoop_joy nodes:

<?xml version="1.0" ?> 
<launch> 
  <node pkg="chapter8_tutorials" type="c8_teleop_joy" 
   name="c8_teleop_joy" /> 
  <param name="axis_linear" value="1" type="int" /> 
  <param name="axis_angular" value="0" type="int" /> 
  <node respawn="true" pkg="joy" type="joy_node" name="joy_node"> 
    <param name="dev" type="string" value="/dev/input/js0" /> 
    <param name="deadzone" value="0.12" /> 
  </node> 
</launch> 

There are four parameters in the launch file; these parameters will add data to Parameter Server, and it will be used by our node. The axis_linear and axis_angular parameters will be used to configure the axes of the joystick. If you want to change the axes configuration, you only need to change the value and put the number of the axes you want to use. The dev and deadzone parameters will be used to configure the port where the joystick is connected, and the dead zone is the region of movement that is not recognized by the device.

    $ roslaunch chapter8_tutorials chapter8_teleop_joy.launch
  

You can check that the code is working using rostopic echo that /cmd_vel topic is published when you move the joystick axes.

Now, we are going to prepare a larger launch to visualize a robot model that moves with the joystick. Copy the following code step to a new file, chapter8_tutorials_robot_model.launch, in the chapter8_tutorials/launch directory:

<?xml version="1.0"?> 
<launch> 
  <arg name="model" /> 
  <arg name="gui" default="False" /> 
  <param name="robot_description" textfile="$(find 
   chapter8_tutorials)/urdf/robot2.urdf" /> 
  <param name="use_gui" value="$(arg gui)" /> 
  <node name="joint_state_publisher" pkg="joint_state_publisher" 
   type="joint_state_publisher" />  <node name="robot_state_publisher" pkg="robot_state_publisher" 
   type="state_publisher" />  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find 
   chapter8_tutorials)/config/config.rviz" /> 
  <node name="c8_odom" pkg="chapter8_tutorials" type="c8_odom" /> 
  <node name="joy_node" pkg="joy" type="joy_node" /> 
  <node name="c8_teleop_joy" pkg="chapter8_tutorials" 
   type="c8_teleop_joy" /> 
</launch> 

You will note that, in the launch file, there are four different nodes: c8_teleop_joy, joy_node, c8_odom, joint_state_publisher, and rviz. You can note in the launch file that the robot model is named robot2.urdf and you can find it in chapter8_tutorials/urdf.

To launch the example, use the following command line:

    $ roslaunch chapter8_tutorials chapter8_robot_model.launch  

You can see whether everything is fine by checking the running nodes and the
topic list using the rosnode and rostopic lists. If you want to see it graphically, use rqt_graph.

If all has been successful, you should see in the RViz visualizer a robot model that you can control with the joystick!

In the next sections, we will learn how to use Arduino with ROS. Then, we will connect the joystick example nodes to Arduino node to control real robot motors.