ROS:Tutorials/Control Your Robot

From TelerobcarWiki
Jump to: navigation, search

Description: In this tutorial you will control your robot within Gazebo.

Tutorial Level: INTERMEDIATE

Next Tutorial: Haptic Controller

Changes in our xacro or urdf files

  • transmissions block: We have to add a transmission block for each joint we want to controll it. In this case, we have 4 transmission blocks, one for each wheel.
    <!-- Transmission is important to link the joints and the controller -->
    <transmission name="${fb}_${lr}_wheel_joint_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="${fb}_${lr}_wheel_joint" />
      <actuator name="${fb}_${lr}_wheel_joint_motor">
        <hardwareInterface>EffortJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
    </transmission>
  • control plugin: We need a plugin to control our robot. The basic control plugin is:
<gazebo>
  <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    <robotNamespace>/MYROBOT</robotNamespace>
  </plugin>
</gazebo>

But, to control our mobile robot, we use this:

  <gazebo>
    <plugin name="skid_steer_drive_controller" filename="libgazebo_ros_skid_steer_drive.so">
      <alwaysOn>true</alwaysOn>
      <updateRate>100.0</updateRate>
      <leftFrontJoint>front_left_wheel_joint</leftFrontJoint>
      <rightFrontJoint>front_right_wheel_joint</rightFrontJoint>
      <leftRearJoint>back_left_wheel_joint</leftRearJoint>
      <rightRearJoint>back_right_wheel_joint</rightRearJoint>
      <wheelSeparation>${base_y_size}</wheelSeparation>
      <wheelDiameter>${2*wheel_radius}</wheelDiameter>
      <torque>35</torque>
      <broadcastTF>1</broadcastTF>
      <odometryFrame>map</odometryFrame>
      <commandTopic>cmd_vel</commandTopic>
      <odometryTopic>odom</odometryTopic>
      <robotBaseFrame>base_footprint</robotBaseFrame>
    </plugin>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so" />
  </gazebo>


Creating a ROS controller

Now is the moment to move our robot, but first we have to create and configure the controllers.

Create new package

cd ~/catkin_ws/src/labrob/
catkin_create_pkg labrob_control ros_control ros_controllers controller_manager geometry_msgs joy sensor_msgs std_msgs
cd labrob_control
mkdir config

Create a .yaml config file

The PID gains and controller settings must be saved in a yaml file that gets loaded to the param server via the roslaunch file.

roscd labrob_control
geany config/joint_position_control.yaml

This would be an .yaml example for a position control

rrbot:
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  
 
  # Position Controllers ---------------------------------------
  joint1_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint1
    pid: {p: 100.0, i: 0.01, d: 10.0}
  joint2_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint2
    pid: {p: 100.0, i: 0.01, d: 10.0}

In our case, we are using a different control plugin, so our .yaml file will be:

  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50

Create a roslaunch file

You can create a roslaunch file to load the controllers, or add some lines in your old launch file. In this case, we are going to modify our labrob_gazebo/launch/labrob_gazebo.launch:

<launch>
  <!-- roslaunch arguments -->
  <arg name="show_rviz" default="true"/>
  <arg name="paused" default="false"/>
  <arg name="debug" default="false"/>  
  <arg name="gui" default="true"/>
 
  <!-- We resume the logic in empty_world.launch, changing only the name of 
    the world to be launched -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="worlds/empty.world" />
    <arg name="paused" value="$(arg paused)" />
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />    
    <arg name="use_sim_time" value="true" />
    <arg name="headless" value="false" />
  </include>
 
  <!-- urdf xml robot description loaded on the Parameter Server-->
  <param name="robot_description" command="$(find xacro)/xacro.py '$(find labrob_description)/urdf/labrob.urdf.xacro'" />
 
  <!-- push robot_description to factory and spawn robot in gazebo -->
  <node name="labrob_spawn" pkg="gazebo_ros" type="spawn_model" output="screen" 
    args="-urdf -param robot_description -model labrob" />
 
  <!-- robot visualization in Rviz -->
  <group if="$(arg show_rviz)">
    <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find labrob_gazebo)/launch/gazebo.rviz"/> 
 
    <!-- load joint controller configurations from YAML file to parameter server -->
    <rosparam file="$(find labrob_control)/config/joint_position_control.yaml" command="load" />
    <!-- load the controllers -->
    <node name="labrob_controller" pkg="controller_manager" type="spawner" output="screen" 
      args="joint_state_controller" />
 
    <!-- publish all the frames to TF -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
      <param name="publish_frequency" value="50"/> <!-- Hz -->
    </node>
  </group>
 
</launch>

Now, we are going to explain the new launch file:

  • We have added a new argument to specify if we want to show rviz or not
  <!-- roslaunch arguments -->
  <arg name="show_rviz" default="true"/>
  • This is the conditional block for rviz
  <!-- robot visualization in Rviz -->
  <group if="$(arg show_rviz)">
    <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find labrob_gazebo)/launch/gazebo.rviz"/> 
 
    <!-- load joint controller configurations from YAML file to parameter server -->
    <rosparam file="$(find labrob_control)/config/joint_position_control.yaml" command="load" />
    <!-- load the controllers -->
    <node name="labrob_controller" pkg="controller_manager" type="spawner" output="screen" 
      args="joint_state_controller" />
 
    <!-- publish all the frames to TF -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
      <param name="publish_frequency" value="50"/> <!-- Hz -->
    </node>
  </group>
  • Within this block, we load the rviz node
  <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find labrob_gazebo)/launch/gazebo.rviz"/>
  • Load the controllers
    <!-- load joint controller configurations from YAML file to parameter server -->
    <rosparam file="$(find labrob_control)/config/joint_position_control.yaml" command="load" />
    <!-- load the controllers -->
    <node name="labrob_controller" pkg="controller_manager" type="spawner" output="screen" 
      args="joint_state_controller" />
  • Convert joint states to TF transforms for rviz, etc
    <!-- publish all the frames to TF -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
      <param name="publish_frequency" value="50"/> <!-- Hz -->
    </node>

Controlling the mobile robot

roslaunch labrob_gazebo labrob_gazebo.launch

With rqt

If you don't have rqt installed, you have to do it:

sudo apt-get install ros-hydro-rqt ros-hydro-rqt-common-plugins ros-hydro-rqt-robot-plugins
sudo apt-get update
sudo apt-get dist-upgrade

Open rqt in other terminal

rqt

Then, you have to click in Plugins-> Robot Tools -> Robot steering.

In this gui, you will have two sliders, lineal and angular velocity. You have to publish that information in /cmd_vel topic to move your robot.

With key_teleop

You can run a python script to control the robot with the keyboard.

roscd labrob_control
mkdir scripts
touch scripts/key_teleop.py
chmod +x scripts/key_teleop.py
geany scripts/key_teleop.py
#! /usr/bin/env python
import rospy, math
import numpy as np
import sys, termios, tty, select, os
from geometry_msgs.msg import Twist
 
class KeyTeleop(object):
  cmd_bindings = {'q':np.array([1,1]),
                  'w':np.array([1,0]),
                  'e':np.array([1,-1]),
                  'a':np.array([0,1]),
                  'd':np.array([0,-1]),
                  'z':np.array([-1,-1]),
                  'x':np.array([-1,0]),
                  'c':np.array([-1,1])
                  }
  set_bindings = { 't':np.array([1,1]),
                  'b':np.array([-1,-1]),
                  'y':np.array([1,0]),
                  'n':np.array([-1,0]),
                  'u':np.array([0,1]),
                  'm':np.array([0,-1])
                }
  def init(self):
    # Save terminal settings
    self.settings = termios.tcgetattr(sys.stdin)
    # Initial values
    self.inc_ratio = 0.1
    self.speed = np.array([0.5, 1.0])
    self.command = np.array([0, 0])
    self.update_rate = 10   # Hz
    self.alive = True
    # Setup publisher
    self.pub_twist = rospy.Publisher('/cmd_vel', Twist)
 
  def fini(self):
    # Restore terminal settings
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
 
  def run(self):
    try:
      self.init()
      self.print_usage()
      r = rospy.Rate(self.update_rate) # Hz
      while not rospy.is_shutdown():
        ch = self.get_key()
        self.process_key(ch)
        self.update()
        r.sleep()
    except rospy.exceptions.ROSInterruptException:
      pass
    finally:
      self.fini()
 
  def print_usage(self):
    msg = """
    Keyboard Teleop that Publish to /cmd_vel (geometry_msgs/Twist)
    Copyright (C) 2013
    Released under BSD License
    --------------------------------------------------
    H:       Print this menu
    Moving around:
      Q   W   E
      A   S   D
      Z   X   Z
    T/B :   increase/decrease max speeds 10%
    Y/N :   increase/decrease only linear speed 10%
    U/M :   increase/decrease only angular speed 10%
    anything else : stop
 
    G :   Quit
    --------------------------------------------------
    """
    self.loginfo(msg)
    self.show_status()
 
  # Used to print items to screen, while terminal is in funky mode
  def loginfo(self, str):
    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
    print(str)
    tty.setraw(sys.stdin.fileno())
 
  # Used to print teleop status
  def show_status(self):
    msg = 'Status:\tlinear %.2f\tangular %.2f' % (self.speed[0],self.speed[1])
    self.loginfo(msg)
 
  # For everything that can't be a binding, use if/elif instead
  def process_key(self, ch):
    if ch == 'h':
      self.print_usage()
    elif ch in self.cmd_bindings.keys():
      self.command = self.cmd_bindings[ch]
    elif ch in self.set_bindings.keys():
      self.speed = self.speed * (1 + self.set_bindings[ch]*self.inc_ratio)
      self.show_status()     
    elif ch == 'g':
      self.loginfo('Quitting')
      # Stop the robot
      twist = Twist()
      self.pub_twist.publish(twist)
      rospy.signal_shutdown('Shutdown')
    else:
      self.command = np.array([0, 0])
 
  def update(self):
    if rospy.is_shutdown():
      return
    twist = Twist()
    cmd  = self.speed*self.command
    twist.linear.x = cmd[0]
    twist.angular.z = cmd[1]
    self.pub_twist.publish(twist)
 
  # Get input from the terminal
  def get_key(self):
    tty.setraw(sys.stdin.fileno())
    select.select([sys.stdin], [], [], 0)
    key = sys.stdin.read(1)
    return key.lower()
 
if __name__ == '__main__':
  rospy.init_node('keyboard_teleop')
  teleop = KeyTeleop()
  teleop.run()

FALTA EXPLICACIÓN


Then you only have to run this script

rosrun labrob_control key_teleop.py


With joy_teleop

You can use a joystick or a gamepad to control the robot

First, you have to install the joy package

sudo apt-get install ros-hydro-joystick-drivers

Later, connect your gamepad or joystick, test it (usually it is in /dev/input/js0) and give it reading and execution permissions.

ls /dev/input/
sudo jstest /dev/input/js0
sudo chmod a+rw /dev/input/js0

If your gamepad path isn't /dev/input/js0, change the instructions for the correct path

sudo jstest /dev/input/jsX
sudo chmod a+rw /dev/input/jsX
rosparam set joy_node/dev "/dev/input/jsX"

Now, we are going to create our script joy_teleop.py

roscd labrob_control
mkdir scripts
touch scripts/joy_teleop.py
chmod +x scripts/joy_teleop.py
geany scripts/joy_teleop.py
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Joy
 
def callback(data):
	twist = Twist()
	twist.linear.x = 4*data.axes[1]
	twist.angular.z = 4*data.axes[2]
	pub.publish(twist)
 
def joy_teleop():
	rospy.init_node('Joy_teleop')
	rospy.Subscriber("joy", Joy, callback)
	global pub
	pub = rospy.Publisher('/cmd_vel', Twist)
 
	r = rospy.Rate(10) # 10hz
	while not rospy.is_shutdown():
		r.sleep()
 
	# spin() simply keeps python from exiting until this node is stopped
	rospy.spin()
 
if __name__ == '__main__':
	joy_teleop()

Explanation:

  • Import msg types
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Joy
  • Init the node, subscribe to joy node to receive joystick information (when joy_teleop node receives a new msg, the callback method will be invoked), and publish in /cmd_vel to control the robot.
def joy_teleop():
	rospy.init_node('Joy_teleop')
	rospy.Subscriber("joy", Joy, callback)
	global pub
	pub = rospy.Publisher('/cmd_vel', Twist)
  • Configure a while block, to wait joy messages while the node is running
	r = rospy.Rate(10) # 10hz
	while not rospy.is_shutdown():
		r.sleep()
 
	# spin() simply keeps python from exiting until this node is stopped
	rospy.spin()
  • When callback is invoked, a Twist msg is created and published. You can choose what axes or buttons you are going to use, in this case, we will use the axes[1] and the axes[0].
def callback(data):
	twist = Twist()
	twist.linear.x = 4*data.axes[1]
	twist.angular.z = 4*data.axes[0]
	pub.publish(twist)

At the end, run the nodes

rosrun joy joy_node
rosrun labrob_control joy_teleop.py


Add a camera

If we can get images from the robot enviroment, we must configure a camera plugin.

First, we add a camera_link in our xacro or urdf file:

  <property name="camera_link" value="0.05" /> <!-- Size of square 'camera' box -->
  ...
  <!-- Camera -->
  <joint name="camera_joint" type="fixed">
    <axis xyz="0 1 0" />
    <origin xyz="${base_x_size/2-camera_link/2} 0 ${base_z_size/2 + camera_link/2}" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="camera_link"/>
  </joint>
 
  <link name="camera_link">
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
		<box size="${camera_link} ${camera_link} ${camera_link}"/>
      </geometry>
    </collision>
 
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
		<box size="${camera_link} ${camera_link} ${camera_link}"/>
      </geometry>
      <material name="black"/>
    </visual>
 
    <inertial>
      <mass value="1e-5" />
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
    </inertial>
  </link>

We also add the plugin to receive images:

  <gazebo reference="camera_link">
    <sensor type="camera" name="camera1">
      <update_rate>30.0</update_rate>
      <camera name="head">
        <horizontal_fov>1.3962634</horizontal_fov>
        <image>
          <width>800</width>
          <height>800</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <!-- Noise is sampled independently per pixel on each frame.  
               That pixel's noise value is added to each of its color
               channels, which at that point lie in the range [0,1]. -->
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
      </camera>
      <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>0.0</updateRate>
        <cameraName>labrob/camera</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>camera_link</frameName>
        <hackBaseline>0.07</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
  </gazebo>

At the end, test it:

roslaunch labrob_gazebo labrob_gazebo.launch

In other terminal:

rqt

Click Plugins->Visualization->Image View and select the /labrob/camera/image_raw



Next Tutorial: Now that you can control your robot with a keyboard now try with a haptic controller ?