If you are reading this, it is because you either are or you want to become a ROS developer. Basically a ROS developer is somebody who wants to build programs for robots based on ROS.
Yeah, but what is ROS? How can I learn ROS? How can I start developing with it? What is the most optimal development environment for it? All those questions will be answered in the following pages.
But before we start with the practical things, let’s answer some basic questions about what ROS is and why it can be the door through which you can become a developer for robots.
WHY ARE THERE NOT ENOUGH DEVELOPERS FOR ROBOTICS?
In general, software developers do not like to deal with hardware.
It is very likely that you are a developer and never thought about entering the robotics realm. You probably think that by programming for robots, you would need to know about electronics and maybe even mechanics. You probably think that hardware and software are too coupled in robots, and that you cannot touch one thing without touching the other.
For example, some years ago, I had to make the navigation system for a robot. However, our navigation program was not working at all. We thought that it was something wrong with the program, but after extensive review, we found that it actually was a problem with the electronics of the laser scanner that we used to localize the robot. There were some micro-interruptions in the voltage level that made the laser reboot. In order to find that error, we had to go to the basics and find where the problem within the physical laser was. For that, you need to mess with the electronics. You have to take the laser out of the robot, put it on your table, and start experimenting. Different voltages, different interruptions to the power, all in order to try to reproduce the effect in a controlled environment. That is a lot of interaction with the hardware.
Interaction with hardware is something that many software developers don’t like. After all, they decided to become developers of software, not hardware!!
ROBOTICISTS PROGRAMMING ROBOTS
Due to that, the programming of robots has been done by roboticists, who are the people that build the robots. Maybe some of them are not directly involved in the creation of the robot, but definitely they have no problem getting into the hardware and trying to fix some hardware problems, in order to make their program work.
But let’s face it, most roboticists are better developers than programmers. That is why robotics could benefit so much from having lots of expert programmers coming to the field.
The good news is that getting developers into the field is more possible than ever. Thanks to the Robot Operating System, ROS, you can completely abstract the hardware from the software, so you can program a robot just by knowing the robot ROS API. By using the ROS API, you can forget about the hardware and just concentrate on the software that makes the robot do what you want.
WHAT IS THE ROBOT ROS API?
The ROS API is the list of ROS topics, services, action servers, and messages that a given robot is providing to give access to its hardware, that is, sensors and actuators. If you are not familiar with ROS, you may not understand what those terms mean. But simply put in the developers’ language, topics/services/messages are like the software functions you can call on a robot to get data from the sensors or make the robot take action. It also includes the parameters you can pass on to those functions.
Most modern robot builders are providing off-the-shelf ROS APIs, like for example, ROS- Components shop that provides all its hardware running with a ROS API.
If the robot you want to work with does not run ROS, you can still make the robot work with ROS by ROSifying it. To ROSify means to adapt your robot to work with ROS. To ROSify a robot usually requires knowledge to access the hardware. You need to learn how to communicate with the electronics that provide the sensor data or access the motors of the robot. In this series of ROS tutorials, we are not dealing with that subject because it gets out of scope for developers. But if you are interested in this topic, you can learn more about it in this Robot Creation Course.
So for the rest of the tutorials, we will assume that you have access (or are willing to have access) to a robot that is ROSified.
WHAT IS ROS ANYWAY?
ROS stands for Robot Operating System. Even if it says so, ROS is not a real operating system since it goes on top of Linux Ubuntu. ROS is a framework on top of the O.S. that allows it to abstract the hardware from the software. This means you can think in terms of software for all the hardware of the robot. And that’s good news for you because this implies that you can actually create programs for robots without having to deal with the hardware. Yeah!
ROS FOR SERVICE ROBOTS
ROS is becoming the standard in robotics programming, at least in the service robots sector. Initially, ROS started at universities, but quickly spread into the business world. Every day, more and more companies and startups are basing their businesses in ROS.
Before ROS, every robot had to be programmed using the manufacturer’s own API. This means that if you changed robots, you had to start the entire software again, apart from having to learn the new API. Furthermore, you had to know a lot about how to interact with the electronics of the robot in order to understand how your program was doing. The situation was similar to that of computers in the 80s, when every computer had its own operating system and you had to create the same program for every type of computer. ROS is for robots like Windows is for PCs, or Android for phones.
By having a ROSified robot, that is, a robot that runs on ROS, you can create programs that can be shared among different robots. You can build a navigation program, that is a program to make a robot move around without colliding, for a four-wheeled robot built by company A and then use the same exact code to move a two-wheeled robot built by company B… or even use it on a drone from company C.
SOME COMMONLY ASKED QUESTIONS
Which operating system should be used with ROS?
ROS works on Linux Ubuntu or Linux Debian. Experimental support already exists for OSX and Gentoo, and a version for Windows in underway, but we really don’t recommend for you to use them yet, unless you are an expert. Check this page for more information about how to use ROS on those systems.
If you don’t know how to work with Linux, I recommend you start with this free Linux for Robotics course.
HOW TO DEVELOP FOR ROBOTS WITH ROS
Now, if you are convinced you want to become a robotics developer, in this series of ROS tutorials, you are going to find the steps that you can take to become a ROS developer. We have divided the tutorial series into the following sections that should cover the whole development process:
• Setting Up• Learning• Coding• Testing
Let’s go, one by one.
[irp posts=”14751″ name=”ROS for Beginners: How to Learn ROS”]
To access the full guide, click the orange button below, or go to RobotIgniteAcademy.com.
This part is what is used in the Python script that calculates the distance moved.
Step 4: Understand the Python scripts
1. Open the IDE and browse to the src folder. You will find two packages odom_movement_detector and test_movement. Each of these projects contains a Python script, each explained with comments in the code
#!/usr/bin/env python
import rospy
import math
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point
from std_msgs.msg import Float64
class MovementDetector(object):
def __init__(self):
"""Initialize an object of the MovementDetector class."""
# _mved_distance is for stored distance moved
# create and initialize it here. Initial value is 0.0
self._mved_distance = Float64()
self._mved_distance.data = 0.0
# Get the inital position. This will be a reference point for calculating
# the distance moved
self.get_init_position()
# Create a publisher for publishing the distance moved into the topic '/moved_distance'
self.distance_moved_pub = rospy.Publisher('/moved_distance', Float64, queue_size=1)
# create a subscriber for getting new Odometry messages
rospy.Subscriber("/odom", Odometry, self.odom_callback)
def get_init_position(self):
"""Get the initial position of the robot."""
"""
Structure of the odom position message:
user:~$ rostopic echo /odom -n1
header:
seq: 14929
stamp:
secs: 748
nsecs: 215000000
frame_id: "odom"
child_frame_id: "base_footprint"
pose:
pose:
position:
x: 0.00668370211388
y: 0.00010960687178
z: -0.000246865753431
"""
data_odom = None
# wait for a message from the odometry topic and store it in data_odom when available
while data_odom is None:
try:
data_odom = rospy.wait_for_message("/odom", Odometry, timeout=1)
except:
rospy.loginfo("Current odom not ready yet, retrying for setting up init pose")
# Store the received odometry "position" variable in a Point instance
self._current_position = Point()
self._current_position.x = data_odom.pose.pose.position.x
self._current_position.y = data_odom.pose.pose.position.y
self._current_position.z = data_odom.pose.pose.position.z
def odom_callback(self, msg):
"""Process odometry data sent by the subscriber."""
# Get the position information from the odom message
# See the structure of an /odom message in the `get_init_position` function
NewPosition = msg.pose.pose.position
# Calculate the new distance moved, and add it to _mved_distance and
self._mved_distance.data += self.calculate_distance(NewPosition, self._current_position)
# Update the current position of the robot so we have a new reference point
# (The robot has moved and so we need a new reference for calculations)
self.updatecurrent_positin(NewPosition)
# If distance moved is big enough, publish it to the designated topic
# Otherwise publish zero
if self._mved_distance.data < 0.000001:
aux = Float64()
aux.data = 0.0
self.distance_moved_pub.publish(aux)
else:
self.distance_moved_pub.publish(self._mved_distance)
def updatecurrent_positin(self, new_position):
"""Update the current position of the robot."""
self._current_position.x = new_position.x
self._current_position.y = new_position.y
self._current_position.z = new_position.z
def calculate_distance(self, new_position, old_position):
"""Calculate the distance between two Points (positions)."""
x2 = new_position.x
x1 = old_position.x
y2 = new_position.y
y1 = old_position.y
dist = math.hypot(x2 - x1, y2 - y1)
return dist
def publish_moved_distance(self):
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
if __name__ == '__main__':
# create a node for running the program
rospy.init_node('movement_detector_node', anonymous=True)
# create an instance of the MovementDetector class and set the code
# in motion
movement_obj = MovementDetector()
movement_obj.publish_moved_distance()
test_movement/src/test_movement.py
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import Float64
def my_callback(msg):
"""Callback function that processes messages from the subscriber."""
# get the distance moved from the message
distance_moved = msg.data
# If distance is less than 2, continue moving the robot
# Otherwise, stop it (by pubishing `0`)
if msg.data < 2:
move.linear.x = 0.1
if msg.data > 2:
move.linear.x = 0
pub.publish(move)
# create a node for running the program
rospy.init_node('test_movement')
# create a subscriber that gets the distance moved
sub = rospy.Subscriber('/moved_distance', Float64, my_callback)
# Create a publisher that moves the robot
pub = rospy.Publisher('/cmd_vel', Twist, queue_size="1")
# Create a global variable for publising a Twist ("cmd_vel") message
move = Twist()
# Keep the program running
rospy.spin()
Here below you have a “sights and sounds” version of this post, just in case you prefer it that way. Enjoy!
Feedback
Did you like this post? Do you have any questions about the explanations? Whatever the case, please leave a comment on the comments section below, so we can interact and learn from each other.
If you want to learn about other ROS or ROS2 topics, please let us know in the comments area and we will do a video or post about it.
In the previous post, we learned how to add pressure sensors in a gazebo simulation. Now we are going to learn how to see then on RViz (ROS Visualization tool). Remember that there is a question on ROS Answers about contact sensors, which is what originated this post.
Start the provided ROSject
The first thing you need to do is to have a copy of the ROSject we mentioned above if you want everything ready to go. You can also clone the git repository aforementioned if you want to run everything in your local computer. If you for the ROSject option, just click on the ROSject link to get an automatic copy. You should now see a ROSject called DogBotTactileSensors on your list of ROSjects, something like the image below:
DogBot tactile sensors in ROSDS
After clicking on the Open button to open the ROSject, you should have the ROSject opened in a remote computer launched on ROSDS.
Launching the simulation
If you remember the previous post, we can launch the simulation by running roslaunch dogbot_gazebo main.launch on the webshell. You can also use the menu by clicking on Simulations -> Choose Simulation -> Choose Launch File and selecting the main.launch file in the dogbot_gazebo package:
dogbot_gazebo main.launch in ROSDS
You should now have DogBot up and running in ROSDS:
Adding the sensors to RViz
Let’s start by stopping the robot. For that, we can just run the keyboard teleoperation with rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=/dogbot/cmd_vel to move DogBot and then hit K to stop it.
Now we open a new shell by clicking on Tools -> Shell and run:
rosrun dogbot_markers arrows_rviz.py
This command essentially get the data published by the sensors and convert then into markers that can be shown in RViz.
Now, to see RViz we have to launch by running the command below in a new shell:
rosrun rviz rviz
This will open RViz, but in order to see it, you have to open the Graphical Tools by clicking Tools -> Graphical Tools.
Launching Graphical Tools in ROSDS
Once you are in RViz, you open a config file by clicking File -> Open Config. There you select the file dogbot.rviz file located in ~/simulation_ws/src/dogbot_tc/dogbot_markers/rviz/dogbot.rviz
Select the rviz config file dogbot.rviz for dogbot
The file you open has everything configured to show the contact markers in RViz.
The pressure in each of the feet of Dogbit is represented by an arrow, which lengths and colors are proportional to the pressure registered. You can see the arrows changing their lengths when the robot steps.
How the markers are created
On the folder ~/simulation_ws/src/dogbot_tc/dogbot_markers/scripts we created a script called world_to_base_tf_publisher.py with the following content:
#! /usr/bin/env python
import rospy
import time
import tf
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Pose
class WorldBaseTFPublisher(object):
def __init__(self):
rospy.loginfo("Start init WorldBaseTFPublisher Class")
self._br = tf.TransformBroadcaster()
rospy.loginfo("set up tf.TransformBroadcaster DONE")
self._current_pose = Pose()
rospy.loginfo("_current_pose DONE")
self.get_init_position()
rospy.loginfo("get_init_position DONE")
self._sub = rospy.Subscriber('/dogbot/odom', Odometry, self.odom_callback)
rospy.loginfo("self._sub DONE")
def get_init_position(self):
data_odom = None
r = rospy.Rate(2)
while data_odom is None and not rospy.is_shutdown():
try:
data_odom = rospy.wait_for_message("/dogbot/odom", Odometry, timeout=1)
except:
rospy.loginfo("Current odom not ready yet, retrying for setting up init pose")
try:
r.sleep()
except rospy.ROSInterruptException:
# This is to avoid error when world is rested, time when backwards.
pass
self._current_pose = data_odom.pose.pose
def odom_callback(self, msg):
self._current_pose = msg.pose.pose
def get_current_pose(self):
return self._current_pose
def handle_turtle_pose(self, pose_msg, link_name, world_name = "/world"):
self._br.sendTransform(
(pose_msg.position.x,
pose_msg.position.y,
pose_msg.position.z),
(pose_msg.orientation.x,
pose_msg.orientation.y,
pose_msg.orientation.z,
pose_msg.orientation.w),
rospy.Time.now(),
link_name,
world_name)
def publisher_of_tf(self):
frame_link_name = "base_link"
time.sleep(1)
rospy.loginfo("Ready..Starting to Publish TF data now...")
rate = rospy.Rate(50)
while not rospy.is_shutdown():
pose_now = self.get_current_pose()
if not pose_now:
print "The Pose is not yet available...Please try again later"
else:
self.handle_turtle_pose(pose_now, frame_link_name)
try:
rate.sleep()
except rospy.ROSInterruptException:
# This is to avoid error when world is rested, time when backwards.
pass
if __name__ == '__main__':
rospy.init_node('publisher_of_world_base_tf_node', anonymous=True)
rospy.loginfo("STARTING WORLS TO BASE TF PUBLISHER...")
world_base_tf_pub = WorldBaseTFPublisher()
world_base_tf_pub.publisher_of_tf()
In this file we get the /dogbot/odom topic, which informs where is the robot in the world, and publishes the tf from world to base_link. With the command rosrun tf view_frames, we generate the frames.pdf file. By downloading that file we can see that the world is connected to base_link, which is the base of the robot.
tf world connected to base_link frame on ROSDS
The connection between the two frames is important because the sensors are represented in the world frame, but are detected by the robot, which is represented by the base_link. If you want to learn more about tf, please consider taking the course TF ROS 101 in Robot Ignite Academy.
The markers are created in the script ~/simulation_ws/src/dogbot_tc/dogbot_markers/scripts/arrows_rviz.py, which has the following content:
#!/usr/bin/env python
import rospy
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
import tf
import numpy
from std_msgs.msg import String
from gazebo_msgs.msg import ContactsState
import math
class MarkerBasics(object):
def __init__(self, topic_id):
marker_topic = '/marker_basic_'+topic_id
self.marker_objectlisher = rospy.Publisher(marker_topic, Marker, queue_size=1)
self.rate = rospy.Rate(25)
self.init_marker(index=0)
def init_marker(self, index=0):
self.marker_object = Marker()
self.change_frame(frame="/world", ns="dogbot", index=0)
self.marker_object.type = Marker.ARROW
self.marker_object.action = Marker.ADD
self.change_position(x=0.0, y=0.0, z=0.0)
self.change_orientation(pitch=0.0, yaw=0.0)
self.change_scale()
self.change_colour(R=1.0, G=0.0, B=0.0)
# If we want it for ever, 0, otherwise seconds before desapearing
self.marker_object.lifetime = rospy.Duration(0)
def change_orientation(self, pitch, yaw):
"""
Roll doesnt make any sense in an arrow
:param pitch: Up Down. We clip it to values [-1.5708,1.5708]
:param yaw: Left Right , No clamp
:return:
"""
pitch = numpy.clip(pitch, -1.5708,1.5708)
q = tf.transformations.quaternion_from_euler(0.0, pitch, yaw)
self.marker_object.pose.orientation.x = q[0]
self.marker_object.pose.orientation.y = q[1]
self.marker_object.pose.orientation.z = q[2]
self.marker_object.pose.orientation.w = q[3]
def change_position(self, x, y, z):
"""
Position of the starting end of the arrow
:param x:
:param y:
:param z:
:return:
"""
my_point = Point()
my_point.x = x
my_point.y = y
my_point.z = z
self.marker_object.pose.position = my_point
#rospy.loginfo("PositionMarker-X="+str(self.marker_object.pose.position.x))
def change_colour(self, R, G, B):
"""
All colours go from [0.0,1.0].
:param R:
:param G:
:param B:
:return:
"""
self.marker_object.color.r = R
self.marker_object.color.g = G
self.marker_object.color.b = B
# This has to be, otherwise it will be transparent
self.marker_object.color.a = 1.0
def change_scale(self, s_x=1.0, s_y=0.1, s_z=0.1):
"""
:param s_x:
:param s_y:
:param s_z:
:return:
"""
self.marker_object.scale.x = s_x
self.marker_object.scale.y = s_y
self.marker_object.scale.z = s_z
def start(self):
pitch = -0.7
yaw = 0.0
s_x = 1.0
while not rospy.is_shutdown():
#self.change_orientation(pitch=pitch,yaw=yaw)
self.change_scale(s_x=s_x)
self.marker_objectlisher.publish(self.marker_object)
self.rate.sleep()
s_x -= 0.01
def translate(self, value, leftMin, leftMax, rightMin, rightMax):
# Figure out how 'wide' each range is
leftSpan = leftMax - leftMin
rightSpan = rightMax - rightMin
# Convert the left range into a 0-1 range (float)
valueScaled = float(value - leftMin) / float(leftSpan)
# Convert the 0-1 range into a value in the right range.
return rightMin + (valueScaled * rightSpan)
def pressure_to_wavelength_to_rgb(self, pressure, min_pressure=-50.0, max_pressure=50.0, gamma=0.8):
'''This converts a given wavelength of light to an
approximate RGB color value. The wavelength must be given
in nanometers in the range from 380 nm through 750 nm
(789 THz through 400 THz).
Based on code by Dan Bruton
http://www.physics.sfasu.edu/astro/color/spectra.html
'''
wavelength = self.translate(value=pressure,
leftMin=min_pressure, leftMax=max_pressure,
rightMin=380, rightMax=750)
wavelength = float(wavelength)
rospy.logdebug("pressure=" + str(pressure))
rospy.logdebug("wavelength="+str(wavelength))
if wavelength >= 380 and wavelength <= 440:
attenuation = 0.3 + 0.7 * (wavelength - 380) / (440 - 380)
R = ((-(wavelength - 440) / (440 - 380)) * attenuation) ** gamma
G = 0.0
B = (1.0 * attenuation) ** gamma
elif wavelength >= 440 and wavelength <= 490:
R = 0.0
G = ((wavelength - 440) / (490 - 440)) ** gamma
B = 1.0
elif wavelength >= 490 and wavelength <= 510:
R = 0.0
G = 1.0
B = (-(wavelength - 510) / (510 - 490)) ** gamma
elif wavelength >= 510 and wavelength <= 580:
R = ((wavelength - 510) / (580 - 510)) ** gamma
G = 1.0
B = 0.0
elif wavelength >= 580 and wavelength <= 645:
R = 1.0
G = (-(wavelength - 645) / (645 - 580)) ** gamma
B = 0.0
elif wavelength >= 645 and wavelength <= 750:
attenuation = 0.3 + 0.7 * (750 - wavelength) / (750 - 645)
R = (1.0 * attenuation) ** gamma
G = 0.0
B = 0.0
else:
R = 0.0
G = 0.0
B = 0.0
return R, G, B
def change_frame(self, frame="/world", ns="dogbot", index=0):
"""
:param frame:
:return:
"""
self.marker_object.header.frame_id = frame
self.marker_object.header.stamp = rospy.get_rostime()
self.marker_object.ns = ns
self.marker_object.id = index
def update_marker(self, frame, ns, index, position, orientation, pressure, min_pressure=0.0, max_pressure=10.0):
"""
:param position: [X,Y,Z] in the world frame
:param pressure: Magnitude
:param orientation: [Pitch,Yaw]
:return:
"""
#self.change_frame(frame=frame, ns=ns, index=index)
self.change_position(x=position[0], y=position[1], z=position[2])
self.change_orientation(pitch=orientation[0], yaw=orientation[1])
self.change_scale(s_x = pressure)
R,G,B = self.pressure_to_wavelength_to_rgb(pressure=pressure,
min_pressure=min_pressure,
max_pressure=max_pressure,
gamma=0.8)
rospy.logdebug("R,G,B=["+str(R)+", "+str(G)+", "+str(B)+"]")
self.change_colour(R=R, G=G, B=B)
self.marker_objectlisher.publish(self.marker_object)
class FootPressureInfo(object):
def __init__(self):
self.min_pressure = 0.0
self.max_pressure = 2.0
self.markerbasics_object_front_left_foot = MarkerBasics(topic_id="front_left_foot")
self.markerbasics_object_front_right_foot = MarkerBasics(topic_id="front_right_foot")
self.markerbasics_object_back_left_foot = MarkerBasics(topic_id="back_left_foot")
self.markerbasics_object_back_right_foot = MarkerBasics(topic_id="back_right_foot")
rospy.Subscriber("/dogbot/back_left_contactsensor_state", ContactsState, self.contact_callback_back_left_foot)
rospy.Subscriber("/dogbot/back_right_contactsensor_state", ContactsState, self.contact_callback_back_right_foot)
rospy.Subscriber("/dogbot/front_left_contactsensor_state", ContactsState, self.contact_callback_front_left_foot)
rospy.Subscriber("/dogbot/front_right_contactsensor_state", ContactsState, self.contact_callback_front_right_foot)
def contact_callback_front_right_foot(self, data):
"""
:param data:
:return:
"""
foot_name = data.header.frame_id
if len(data.states) >= 1:
Fx = data.states[0].total_wrench.force.x
Fy = data.states[0].total_wrench.force.y
Fz = data.states[0].total_wrench.force.z
pressure = math.sqrt(pow(Fx,2)+pow(Fy,2)+pow(Fz,2))
Px = data.states[0].contact_positions[0].x
Py = data.states[0].contact_positions[0].y
Pz = data.states[0].contact_positions[0].z
pressure = pressure / 100.0
# rospy.loginfo(str(foot_name) + "--->pressure =" + str(pressure))
# rospy.loginfo(str(foot_name) + "Point =[" + str(pressure))
index = 1
self.markerbasics_object_front_right_foot.update_marker(frame=foot_name,
ns="dogbot",
index=index,
position=[Px, Py, Pz],
orientation=[-1.57, 0.0],
pressure=pressure,
min_pressure=self.min_pressure,
max_pressure=self.max_pressure)
else:
# No Contact
pass
def contact_callback_front_left_foot(self, data):
"""
:param data:
:return:
"""
foot_name = data.header.frame_id
if len(data.states) >= 1:
Fx = data.states[0].total_wrench.force.x
Fy = data.states[0].total_wrench.force.y
Fz = data.states[0].total_wrench.force.z
pressure = math.sqrt(pow(Fx,2)+pow(Fy,2)+pow(Fz,2))
Px = data.states[0].contact_positions[0].x
Py = data.states[0].contact_positions[0].y
Pz = data.states[0].contact_positions[0].z
pressure = pressure / 100.0
index = 0
self.markerbasics_object_front_left_foot.update_marker(frame=foot_name,
ns="dogbot",
index=index,
position=[Px, Py, Pz],
orientation=[-1.57, 0.0],
pressure=pressure,
min_pressure=self.min_pressure,
max_pressure=self.max_pressure)
else:
# No Contact
pass
def contact_callback_back_right_foot(self, data):
"""
:param data:
:return:
"""
foot_name = data.header.frame_id
if len(data.states) >= 1:
Fx = data.states[0].total_wrench.force.x
Fy = data.states[0].total_wrench.force.y
Fz = data.states[0].total_wrench.force.z
pressure = math.sqrt(pow(Fx,2)+pow(Fy,2)+pow(Fz,2))
Px = data.states[0].contact_positions[0].x
Py = data.states[0].contact_positions[0].y
Pz = data.states[0].contact_positions[0].z
pressure = pressure / 100.0
#rospy.loginfo(str(foot_name) + "--->pressure =" + str(pressure))
# rospy.loginfo(str(foot_name) + "Point =[" + str(pressure))
index = 2
self.markerbasics_object_back_right_foot.update_marker(frame=foot_name,
ns="dogbot",
index=index,
position=[Px, Py, Pz],
orientation=[-1.57, 0.0],
pressure=pressure,
min_pressure=self.min_pressure,
max_pressure=self.max_pressure)
else:
# No Contact
pass
def contact_callback_back_left_foot(self, data):
"""
:param data:
:return:
"""
foot_name = data.header.frame_id
if len(data.states) >= 1:
Fx = data.states[0].total_wrench.force.x
Fy = data.states[0].total_wrench.force.y
Fz = data.states[0].total_wrench.force.z
pressure = math.sqrt(pow(Fx,2)+pow(Fy,2)+pow(Fz,2))
Px = data.states[0].contact_positions[0].x
Py = data.states[0].contact_positions[0].y
Pz = data.states[0].contact_positions[0].z
pressure = pressure / 100.0
index = 3
self.markerbasics_object_back_left_foot.update_marker(frame=foot_name,
ns="dogbot",
index=index,
position=[Px, Py, Pz],
orientation=[-1.57, 0.0],
pressure=pressure,
min_pressure=self.min_pressure,
max_pressure=self.max_pressure)
else:
# No Contact
pass
if __name__ == '__main__':
rospy.init_node('footpressure_marker_node', anonymous=True)
footpressure_object = FootPressureInfo()
rospy.spin()
When the file is launched, 4 markers are created, each of them publishing on a specific topic. You can find the topic for each marker with rostopic list | grep marker.
We then create the subscribers for the topics, each of then calling a callback function when a topic is published. On the callback we calculate the pressure, get the positions, then publish the marker on RViz.
A video version of this post
So this is the post for today. If you prefer, we also have a video version of this post available in the link below. We hope you liked the post and the video. If so, please feel free to subscribe to our channel, share this post and comment on the video section.
We would like to thank React Robotics for this amazing robot and thank you for reading through this post.
Keep pushing your ROS Learning.
Remember, if you want to learn more about markers in RViz, we highly recommend you taking this course on ROS RViz Advanced Markers.
Interested to learn more? Try this course on URDF creation for Gazebo and ROS:
Learn how to load a pre-built octomap into MoveIt!.
You’ll learn:
Create a Python program for loading an octomap into MoveIt
Set Up MoveIt to detect this octomap
RELATED ROS RESOURCES&LINKS:
ROSject —▸ http://www.rosject.io/l/c52f64c/
ROS Development Studio (ROSDS) —▸ http://rosds.online
Robot Ignite Academy –▸ https://www.robotigniteacademy.com
Related Courses
ROS for Industrial Robots
ROS Manipulation Course
—
Feedback
—
Did you like this video? Do you have questions about what is explained? Whatever the case, please leave a comment on the comments section below, so we can interact and learn from each other.
If you want to learn about other ROS topics, please let us know on the comments area and we will do a video about it 🙂
The first thing you need to do is to have a copy of the ROSject we mentioned above. Click on the link to get an automatic copy. You should now see a ROSject called DogBotTactileSensors on your list of ROSjects, something like the image below:
DogBot tactile sensors in ROSDS
After clicking on the Open button to open the ROSject, you should have the ROSject opened in a remote computer launched on ROSDS.
Launching the simulation
Once the ROSject is open, you can launch a simulation. You can achieve that in two ways:
Opening a web shell (clicking on Tools -> Web Shell) and typing: roslaunch dogbot_gazebo main.launch
Using the menu Simulations -> Choose Simulation -> Choose Launch File. In the end, you will have a menu with a list of launch files. You can select the main.launch file in the dogbot_gazebo package as shown in the image below:
dogbot_gazebo main.launch in ROSDS
You should now have DogBot up and running in ROSDS:
dogbot robot running in ROSDS
In the simulation, you can see the robot has red “shoes” in its feet. There is where we put our pressure sensors.
Launching the simulation in your own local computer
The steps aforementioned are about how to launch the simulation in ROSDS (ROS Development Studio)
If you want to install in your local computer, you can just download the ROSject (or git clone) and execute requirements.sh script, which is located in the simulation_ws/src/dogbot_tc folder with the commands below:
cd simulation_ws/src/dogbot_tc
./requirements.sh
After that, you should be able to launch the simulation with:
source /opt/ros/kinetic/setup.bash
source simulation_ws/devel/setup.bash
roslaunch dogbot_gazebo main.launch
Listing the ROS Topics
Once you have the simulation up and running, you can see the contact sensors with:
rostopic list | grep contact
We can see that the topics are working by subscribing to a contact topic, using the command below:
If we look carefully at the data printed, we can see the frame_id “back_left_foot” and in the states section, you can see the objects the robot is in contact with and the forces applied.
Finding the code
The code used to define the sensors is located in ~/simulation_ws/src/dogbot_tc/dogbot_description/urdf/dogbot.xacro
In ROSDS you can see it easily by using the Code Editor.
dogbot.xacro robot in ROSD
By looking at the code, if you find for “Contact Sensor” you will find it around line 350, inside the <gazebo> tag of the dogbot.xacro file.
Going deeper into the code
On the dogbot.xacro file, the sensor is defined in line 350 like below:
You can see the contact sensors in RViz. For that you can run the following command in a webshell:
rosrun dogbot_markers arrows_rviz.py
In a different shell you run:
rosrun rviz rviz
Open the Graphical Tools and select the rviz file inside the ~/simulation_ws/src/dogbot_tc/dogbot_markers/rviz folder. For that you first need to click Tools -> Graphical Tools, then select the rviz config file.
Launching Graphical Tools in ROSDS
Select the rviz config file for dogbot
dogbot rviz sensors on ROSDS
Here the pressure in each of the feet of Dogbit is represented by an arrow, which lengths and colors are proportional to the pressure registered.
If you want a deeper understanding of how the markers are created and published in RViz, please have a look at the post on adding pressure sensors in RViz.
How to move DogBot
To move the robot you can easily use the command below:
Hit K to make DogBot stop, and to lower the speed, hit Z on the keyboard until when hitting I ( go forwards ) has a sable step. Around 0.1 is ok.
A video version of this post
So this is the post for today. If you prefer, we also have a video version of this post available in the link below. We hope you liked the post and the video. If so, please feel free to subscribe to our channel, share this post and comment on the video section.
We would like to thank React Robotics for this amazing robot and thank you for reading through this post.
Keep pushing your ROS Learning.
Interested in learning more? Try out this course on URDF creation for Gazebo and ROS:
The first thing you need to do is to have a copy of the ROSject we mentioned above. Click on the link to get an automatic copy. You should now see a ROSject called BuoyancyBot on your list of ROSjects, something like the image below:
BuoyancyBot simulation on ROSDS
After clicking on the Open button to open the ROSject, you should have the environment like the one in the image below:
BuoyancyBot opened on ROSDS
Finding the code
After opening the ROSject, you can see the code on the ~/simulation_ws/src folder. There you will find the folders buoyancy_tests_pkg and spawn_robot_tools. It is worth mentioning that the plugin we created is called fluid_resitance.cpp, which has the content below. Bear in mind that you can find that code on the ROSject:
#include "ros/callback_queue.h"
#include "ros/ros.h"
#include "ros/subscribe_options.h"
#include "std_msgs/Float32.h"
#include <functional>
#include <gazebo/common/common.hh>
#include <gazebo/gazebo.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include <ignition/math/Vector3.hh>
#include <thread>
namespace gazebo {
class FluidResitance : public ModelPlugin {
public:
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
if (_sdf->HasElement("fluid_resitanceTopicName")) {
this->fluid_resitanceTopicName =
_sdf->Get<std::string>("fluid_resitanceTopicName");
} else {
ROS_WARN("No Topic Given name given, setting default name %s",
this->fluid_resitanceTopicName.c_str());
}
if (_sdf->HasElement("NameLinkToApplyResitance")) {
this->NameLinkToApplyResitance =
_sdf->Get<std::string>("NameLinkToApplyResitance");
} else {
ROS_WARN("No NameLinkToApplyResitance Given name given, setting default "
"name %s",
this->NameLinkToApplyResitance.c_str());
}
if (_sdf->HasElement("rate")) {
this->rate = _sdf->Get<double>("rate");
} else {
ROS_WARN("No rate Given name given, setting default "
"name %f",
this->rate);
}
// Store the pointer to the model
this->model = _parent;
this->world = this->model->GetWorld();
this->link_to_apply_resitance =
this->model->GetLink(this->NameLinkToApplyResitance);
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
std::bind(&FluidResitance::OnUpdate, this));
// Create a topic name
// std::string fluid_resitance_index_topicName = "/fluid_resitance_index";
// Initialize ros, if it has not already bee initialized.
if (!ros::isInitialized()) {
int argc = 0;
char **argv = NULL;
ros::init(argc, argv, "model_mas_controler_rosnode",
ros::init_options::NoSigintHandler);
}
// Create our ROS node. This acts in a similar manner to
// the Gazebo node
this->rosNode.reset(new ros::NodeHandle("model_mas_controler_rosnode"));
#if (GAZEBO_MAJOR_VERSION >= 8)
this->last_time = this->world->SimTime().Float();
#else
this->last_time = this->world->GetSimTime().Float();
#endif
// Freq
ros::SubscribeOptions so = ros::SubscribeOptions::create<std_msgs::Float32>(
this->fluid_resitanceTopicName, 1,
boost::bind(&FluidResitance::OnRosMsg, this, _1), ros::VoidPtr(),
&this->rosQueue);
this->rosSub = this->rosNode->subscribe(so);
// Spin up the queue helper thread.
this->rosQueueThread =
std::thread(std::bind(&FluidResitance::QueueThread, this));
ROS_WARN("Loaded FluidResitance Plugin with parent...%s, With Fluid "
"Resitance = %f "
"Started ",
this->model->GetName().c_str(), this->fluid_resitance_index);
}
// Called by the world update start event
public:
void OnUpdate() {
float period = 1.0 / this->rate;
// Get simulator time
#if (GAZEBO_MAJOR_VERSION >= 8)
float current_time = this->world->SimTime().Float();
#else
float current_time = this->world->GetSimTime().Float();
#endif
float dt = current_time - this->last_time;
if (dt <= period){
ROS_DEBUG(">>>>>>>>>>TimePassed = %f, TimePeriod =%f ",dt, period);
return;
}else{
this->last_time = current_time;
this->ApplyResitance();
}
}
public:
void SetResitance(const double &_force) {
this->fluid_resitance_index = _force;
ROS_WARN("model_fluid_resitance_index changed >> %f",
this->fluid_resitance_index);
}
void UpdateLinearVel() {
#if (GAZEBO_MAJOR_VERSION >= 8)
this->now_lin_vel = this->model->RelativeLinearVel();
#else
this->now_lin_vel = this->model->GetRelativeLinearVel();
#endif
}
void ApplyResitance() {
this->UpdateLinearVel();
#if (GAZEBO_MAJOR_VERSION >= 8)
ignition::math::Vector3d force, torque;
#else
math::Vector3 force, torque;
#endif
ROS_WARN("LinearSpeed = [%f,%f,%f] ",this->now_lin_vel.x, this->now_lin_vel.y, this->now_lin_vel.z);
force.x = -1.0 * this->fluid_resitance_index * this->now_lin_vel.x;
force.y = -1.0 * this->fluid_resitance_index * this->now_lin_vel.y;
force.z = -1.0 * this->fluid_resitance_index * this->now_lin_vel.z;
// Changing the mass
this->link_to_apply_resitance->AddRelativeForce(force);
#if (GAZEBO_MAJOR_VERSION >= 8)
this->link_to_apply_resitance->AddRelativeTorque(
torque -
this->link_to_apply_resitance->GetInertial()->CoG().Cross(force));
#else
this->link_to_apply_resitance->AddRelativeTorque(
torque -
this->link_to_apply_resitance->GetInertial()->GetCoG().Cross(force));
#endif
ROS_WARN("FluidResitanceApplying = [%f,%f,%f] ",force.x, force.y, force.z);
}
public:
void OnRosMsg(const std_msgs::Float32ConstPtr &_msg) {
this->SetResitance(_msg->data);
}
/// \brief ROS helper function that processes messages
private:
void QueueThread() {
static const double timeout = 0.01;
while (this->rosNode->ok()) {
this->rosQueue.callAvailable(ros::WallDuration(timeout));
}
}
// Pointer to the model
private:
physics::ModelPtr model;
// Pointer to the update event connection
private:
event::ConnectionPtr updateConnection;
// Mas of model
double fluid_resitance_index = 1.0;
/// \brief A node use for ROS transport
private:
std::unique_ptr<ros::NodeHandle> rosNode;
/// \brief A ROS subscriber
private:
ros::Subscriber rosSub;
/// \brief A ROS callbackqueue that helps process messages
private:
ros::CallbackQueue rosQueue;
/// \brief A thread the keeps running the rosQueue
private:
std::thread rosQueueThread;
/// \brief A ROS subscriber
private:
physics::LinkPtr link_to_apply_resitance;
private:
std::string fluid_resitanceTopicName = "fluid_resitance";
private:
std::string NameLinkToApplyResitance = "base_link";
private:
#if (GAZEBO_MAJOR_VERSION >= 8)
ignition::math::Vector3d now_lin_vel;
#else
math::Vector3 now_lin_vel;
#endif
private:
double rate = 1.0;
private:
float last_time = 0.0;
private:
/// \brief The parent World
physics::WorldPtr world;
};
// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(FluidResitance)
} // namespace gazebo
Compile the plugin
Once you have the ROSject, be it on ROSDS or on your own computer, you can easily compile it with:
cd ~/simulation_ws/
catkin_make
Everything should have compiled without any problems. If compiling in your own computer raises errors, please remember running the command source /opt/ros/kinetic/setup.bash before calling catkin_make .
Using the plugin
The plugin we compiled previously has to be loaded by gazebo. We do that on the file
~/simulation_ws/src/buoyancy_tests_pkg/buoyancy_tests_pkg/urdf/simple_floating_sphere_buoyancy_control.urdf, by using the instructions below:
With everything in place, we can now run the simulation. To launch the Simple Fluid Machine, we launch the simulation that has the mass control, the fluid resistance and the propulsion plugins enabled:
If you have applied a movement and you change the FluidResistance, you should see a variation in the time the robot takes to stop. The higher the friction, the faster it will stop its movement.
Air Dragging in Gazebo on ROSDS
There is more to it but have a look at the HectorPluginsAerodinamics, in which this code is based upon.
Change the Mass or buoyancy
By changing the mass, it will change the buoyancy of the model. This is to simulate the water intake of submarines or the buoyancy system that an air flying machine like a blimp could have.
rostopicpub/model_massstd_msgs/Float32"data: 0.0"
Mass_0 = 7.23822947387 –> Neutral buoyancy because its the mass of the object
Mass > Mass_0 –> It will sink
Mass < Mass_0 –> It will rise
This mass you can find it in the URDF of the model: buoyancy_tests_pkg/urdf/simple_floating_sphere_buoyancy_control.urdf
So, that is the post for today. Remember that we also have a live version of this post on our YouTube channel, as can be seen on the link below. If you liked the content of this post or video, please consider subscribing to our channel and pressing the bell for a new video every single day:
![[ROS Q&A] 195 – How to know if robot has moved one meter using Odometry](https://www.theconstruct.ai/wp-content/uploads/2019/09/How-to-know-if-robot-has-moved-1-meter-using-odometry-.png)
![[ROS Q&A] 194 – Add Pressure sensors in RVIZ](https://www.theconstruct.ai/wp-content/uploads/2019/09/ROS-QA-194-Add-Pressure-sensors-in-RVIZ.png)
![[ROS Q&A] 193 – How to load a pre-built octomap into MoveIt](https://www.theconstruct.ai/wp-content/uploads/2019/09/How-to-load-a-pre-built-octomap-into-MoveIt.png)
![[ROS Q&A] 192 – Add Pressure sensors in Gazebo Simulation for DogBot](https://www.theconstruct.ai/wp-content/uploads/2019/09/Add-Pressure-sensors-in-Gazebo-Simulation-for-DogBot.png)
![[ROS Q&A] 190 – Air Drag in Gazebo](https://www.theconstruct.ai/wp-content/uploads/2019/08/Air-Drag-in-Gazebo.png)
