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
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
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.
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
Comments