Robotics StackExchange | Archived questions

Control robot in Gazebo

I've just started to learn Gazebo 7.

I have added a Pioneer 2DX robot and I want to control it. But I don't know how.

I have noticed that the robot has the plugin libDiffDrivePlugin.so. Checking the topics, with gz topic -l, I have found interesting this one:

/gazebo/default/pioneer2dx/vel_cmd

Maybe I can use it to move the robot but I don't know how to publish to it.

I have tried with rqt, using Robot Steering, but it seems that it publishes to /cmd_vel.

How can I move the robot using the keyboard?

Asked by VansFannel on 2018-10-08 14:19:08 UTC

Comments

Answers

To move my custom robot, I had to write a plugin into rqt. I don't know if Pioneer 2DX has any solution ready.

You would need to install QT 4 Designer

$ sudo apt-get install python-qt4 qt4-designer

Then you design your own GUI window image description

Pay attention how you name the buttons. You will use the name to connect the buttons with a specific function. My GUI window has 4 buttons

pushButton_up
pushButton_down
pushButton_left
pushButton_right

a slider on which I can choose the velocity with which I will accelerate the robot

velocitySlider

and a text field

velocityValue

which shows the currently set velocity which will be set to robot if I push the button. You can also write directly into the text field.

image description

I have created a new package just for this feature.

robot_teleop
 ├── CMakeLists.txt
 ├── package.xml
 ├── plugin_description.xml
 ├── resource
 │   └── control.ui
 ├── scripts
 │   └── run_gui_plugin.py
 ├── setup.py
 └── src
     └── robot_teleop
         ├── __init__.py
         └── rqt_plugin.py

The widget from the QT 4 Designer is saved in the /resource/control.ui file.

The CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(robot_teleop)

find_package(catkin REQUIRED COMPONENTS
  rospy
  std_msgs
  rqt_gui
  rqt_gui_py
)

## This macro ensures modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
catkin_python_setup()


###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
  ${catkin_INCLUDE_DIRS}
)

#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
 install(PROGRAMS
   scripts/run_gui_plugin.py
   plugin_description.xml
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )

plugin_description.xml

<library path="src">
    <class name="GUIControllerPlugin"
        type="robot_teleop.rqt_plugin.GUIControllerPlugin"
        base_class_type="rqt_gui_py::Plugin">
        <description>
            A GUI rqt plugin for a manual robot controll.
        </description>
    </class>
</library>

package.xml

<?xml version="1.0"?>
<package format="2">
  <name>robot_teleop</name>
  <version>0.0.0</version>
  <description>rqt plugin for manual control of the robot</description>


  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>

  <build_export_depend>rqt_gui</build_export_depend>
  <build_export_depend>rqt_gui_py</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>

  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>python-rospkg</exec_depend>
  <exec_depend>rqt_gui</exec_depend>
  <exec_depend>rqt_gui_py</exec_depend>



  <export>
    <rqt_gui plugin="${prefix}/plugin_description.xml"/>
  </export>
</package>

setup.py

#!/usr/bin/python

from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup

setup_args = generate_distutils_setup(
     packages=['robot_teleop'],
     package_dir={'': 'src'},
     install_requires=[]
)

setup(**setup_args)

/scripts/run_gui_plugin.py

#!/usr/bin/env python
import sys

from robot_teleop.rqt_plugin import GUIControllerPlugin
from rqt_gui.main import Main

"""
Starts rqt plugin.

run with command:
$ rqt --standalone robot_teleop
"""

main = Main(filename='robot_teleop')
sys.exit(main.main(sys.argv, standalone='robot_teleop'))

The /src/robot_teleop/_init.py_ is an empty file. I suppose it is located where it is to tell catkin where to find the plugin.

And finaly the plugin /src/robot_teleop/rqt_plugin.py

#!/usr/bin/python
import rospy
import os
import rospkg

from qt_gui.plugin import Plugin
from python_qt_binding import loadUi
from python_qt_binding.QtWidgets import QWidget, QMessageBox

from std_msgs.msg import Float64

class GUIControllerPlugin(Plugin):
    """
    rqt plugin for GUI manual control over robot 
    movement. Displays up, down, left and right buttons 
    for direction control and a slider for velocity
    control. 
    """

    def __init__(self, context):
        super(GUIControllerPlugin, self).__init__(context)

        self.setObjectName('GUIControllerPlugin')

        # Process standalone plugin command-line arguments
        from argparse import ArgumentParser
        parser = ArgumentParser()
        # Add argument(s) to the parser.
        parser.add_argument("-q", "--quiet", action="store_true",
                      dest="quiet",
                      help="Put plugin in silent mode")
        args, unknowns = parser.parse_known_args(context.argv())
        if not args.quiet:
            print 'arguments: ', args
            print 'unknowns: ', unknowns

        # Create QWidget
        self._widget = QWidget()

        # Loads the gui window created in QT 4 Designer 
        ui_file = os.path.join(rospkg.RosPack().get_path('robot_teleop'), 'resource', 'control.ui')
        loadUi(ui_file, self._widget)
        self._widget.setObjectName('GUIControllerPluginUI')

        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))

        # Add widget to the user interface
        context.add_widget(self._widget)

        # Wheel joint velocity publishers
        self.publisher_dict = {} 
        # connection initialization
        self.init_signals()
        # publisher initialization
        self.init_ros()
        # set default velocity
        # Specific to my robot. Find out your desired velocity.
        self.velocity = 0    # just to initialize the variable
        self.upper_velocity_limit = 50.0 
        self.update_velocity(10.0)


    def update_velocity(self, vel):
        """
        Sets private velocity, updates slider and
        velocity text field. 
        """
        # the textField will show values from 0 to 100
        # but the real velocity will be 0 to upper_velocity_limit
        self.velocity = vel*( (self.upper_velocity_limit) * 0.01 )
        self._widget.velocityValue.setText( str(vel) )
        self._widget.velocitySlider.setValue(vel)

    def init_signals(self):
        """
        Method connects all nescessary signals from GUI.
        """
        # "pushButton_up" is the name of the button defined in the .ui file
        self._widget.pushButton_up.pressed.connect(self.up_pushed)
        self._widget.pushButton_down.pressed.connect(self.down_pushed)
        self._widget.pushButton_left.pressed.connect(self.left_pushed)
        self._widget.pushButton_right.pressed.connect(self.right_pushed)

        self._widget.pushButton_up.released.connect(self.arrow_released)
        self._widget.pushButton_down.released.connect(self.arrow_released)
        self._widget.pushButton_left.released.connect(self.arrow_released)
        self._widget.pushButton_right.released.connect(self.arrow_released)
                                #sliderMoved
        self._widget.velocitySlider.valueChanged.connect(self.slider_activity)
                                #textChanged editting Finished
        self._widget.velocityValue.textEdited.connect(self.velocity_eddited)


    def init_ros(self):
        """
        Method initialises ROS interaction.
        """
        try:
            # my topics for commanding the velocity
            # insert ('/gazebo/default/pioneer2dx/vel_cmd') for your case
            self.publisher_dict['right'] = rospy.Publisher('/robot/joint_rear_right_wheel_controller/command', Float64, queue_size=1)
            self.publisher_dict['left'] = rospy.Publisher('/robot/joint_rear_left_wheel_controller/command', Float64, queue_size=1)


        except rospy.ROSException as e:
            QMessageBox.about(self._widget, "ROS error", e.message)

    def keyPressEvent(self, event):
         if type(event) == QtGui.QKeyEvent:
             #here accept the event and do something
             print event.key()
             event.accept()
         else:
             event.ignore()

    # The funtions called when button pressed
    def up_pushed(self, checked = False):

        self.publisher_dict['right'].publish(self.velocity)
        self.publisher_dict['left'].publish(self.velocity)

    def down_pushed(self, checked = False):

        self.publisher_dict['right'].publish(-self.velocity)
        self.publisher_dict['left'].publish(-self.velocity)

    def left_pushed(self, checked = False):

        self.publisher_dict['right'].publish(-self.velocity)
        self.publisher_dict['left'].publish(self.velocity)

    def right_pushed(self, checked = False):

        self.publisher_dict['right'].publish(self.velocity)
        self.publisher_dict['left'].publish(-self.velocity)

    def arrow_released(self, checked = False):

        self.publisher_dict['right'].publish(0.0)
        self.publisher_dict['left'].publish(0.0)

    def slider_activity(self):
        """
        Is called when user moves the slider. The set velocity
        is updated into text filed and private velocity variable.
        """
        self.update_velocity( self._widget.velocitySlider.value() )

    def velocity_eddited(self):
        """
        Is called when user edits the velocity text field. The set 
        velocity is updated onto slider and private valocity variable.
        """
        self.update_velocity( float( self._widget.velocityValue.text() ) )

Asked by kumpakri on 2018-10-10 08:07:51 UTC

Comments

Thanks for your answer. So, the only way to control a robot with keyboard in Gazebo is using ROS, isn't it?

Asked by VansFannel on 2018-10-10 10:31:00 UTC

You can define Gazebo plugins for your model. I'm aware that there is a model plugin which will make your robot follow closest object, so there is some possibility to control robot movement within Gazebo itself. I have not come across a Gazebo model plugin, that would allow you to use keyboard or GUI with buttons to control the robot.

Asked by kumpakri on 2018-10-11 05:52:57 UTC

There are also teleop packages available. I didn't try them, but try searching for 'teleop' or 'keyop' or 'keyboard op' or 'keyboard teleop'. Maybe this answer is of interest to you https://answers.ros.org/question/231265/using-keyboard-to-control-the-robot-in-gazebo/ or this answer for gazebo solution only http://answers.gazebosim.org/question/17946/how-can-i-control-my-robot-model-by-gazebo-without-ros-and-its-node/

Asked by kumpakri on 2018-10-12 09:31:36 UTC

hello, thank you for your answer, I am writing a similar controller for my robot following your answer, can you please explain how to execute the plugin after everything has been set up? Thank you

Asked by josejaramillo on 2019-06-04 10:17:05 UTC

@josejaramillo if you meant the rqt plugin, then you need to run a command

rqt --standalone robot_teleop

it will launch the rqt process and load the plugin into it. You might need to add certain flag to the command, if your system is unable to see it at first. The terminal will tell you the flag.

Asked by kumpakri on 2019-06-06 10:32:05 UTC

@kumpakri Thank you very much! works like a champ!

Asked by josejaramillo on 2019-06-19 09:12:00 UTC

Hi I am trying to do the Plugin out using these but there is some problem with my UI file. Does it possible that you can post the Ui folder/ code for me? It will help a lot thanks!!

Asked by potatoo on 2021-01-19 02:36:05 UTC