What will you learn in this post
- How to connect world frame to base_link of DogBot
- How to publish pressure sensors Markers for RVIZ
List of resources used in this post
- The ROSject with the code used in this post.
- Git of DogBot simulation, in case you don’t want to use the ROSject.
- The question on Gazebo Answers
- A live version of this post on YouTube: https://youtu.be/XfPKgWLmx7Y
Overview
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 file world_to_base_tf_publisher.py is automatically launched by put_robot_in_world.launch when we launch the main.launch file.
How the markers are published in RViz
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:
Robot Create with URDF
#dogbot #quadruped #pressuresensors #tactilesensors #rviz #Simulation #Gazebo #Robot
gZfA’) PROCEDURE ANALYSE(EXTRACTVALUE(1580,CONCAT(0x5c,0x716a767071,(SELECT (CASE WHEN (1580=1580) THEN 1 ELSE 0 END)),0x7178767171)),1) AND (‘BoUR’ LIKE ‘BoUR