[ROS Q&A] 194 – Add Pressure sensors in RVIZ

Written by Miguel Angel

12/09/2019

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

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

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

dogbot_gazebo main.launch in ROSDS

You should now have DogBot up and running in ROSDS:

dogbot robot 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

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 for dogbot

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

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

Topics: ROS Q&A | RViz
Masterclass 2023 batch2 blog banner

Check Out These Related Posts

1 Comment

  1. Anonymous

    gZfA’) PROCEDURE ANALYSE(EXTRACTVALUE(1580,CONCAT(0x5c,0x716a767071,(SELECT (CASE WHEN (1580=1580) THEN 1 ELSE 0 END)),0x7178767171)),1) AND (‘BoUR’ LIKE ‘BoUR

    Reply

Submit a Comment

Your email address will not be published.

This site uses Akismet to reduce spam. Learn how your comment data is processed.

Pin It on Pinterest

Share This