ROS Python Classes. Why do we need them

What will you learn in this post

• How to create Python classes in ROS
• When & why do you need these classes in the first place.
• How to spawn a custom simulation in ROS Development Studio (ROSDS) and move it using our python classes.

List of materials with all the content of this post

• ROSject containing all the code shown here
• The ROSject contains the code of the following git: WhyDoINeedThis 

Start the provided ROSject

The first thing you need to do is to copy the ROSject aforementioned. Click on the link to get an automatic copy.

Launch the ROSject

To launch the copied ROSject, go to your ROSDS account and click on the Open button of the copied rosject (see image below). Remember that the rosject you have to open is called WhyINeedIt.

why_python_Classes_rosject

Once the rosject is open, you must see a screen like this:

python_rosject_open

At this point, you have succeeded in launching the ROSject.

Launch the simulation

Let’s launch a simulation because we want a python class that detects when the robot is upside down. For that, let’s do the following:

• Go to Simulations, then with the default empty world on the left, let’s click Choose a robot on the right side, as shown in the image below:

After that, you can type “gurdy” or try to find the Gurdy robot by using the scroll bar. Once the robot is selected, you just click Start Simulation.

You now should see the simulation of the Gurdy robot as in the image below:

Finding the python code

The code used in this post can be found at ~/catkin_ws/src/. You can see the content with:

cd ~/catkin_ws/src/

ls

You can also find the code through the Code Editor. For that, let’s click Tools -> IDE as can be seen on the image below:

Once the IDE is open, we can find all the code that will be used to control the robot inside the catkin_ws folder (catkin workspace).

Detecting when the robot is upside down

In order to detect when the robot is upside down, we have to use the imu topic, which publishes the robot orientation. We can find the topic with the command rostopic list | grep imu :

user:~$ rostopic list | grep imu
/gurdy/imu/data

We see that the topic is named /gurdy/imu/data. We can see what is being published in this topic with rostopic echo -n1 /gurdy/imu/data , as exemplified below:

user:~$ rostopic echo -n1 /gurdy/imu/data
header:
  seq: 0
  stamp:
    secs: 277
    nsecs:  80000000
  frame_id: "base_link"
orientation:
  x: 0.0
  y: 0.0
  z: 0.0
  w: 1.0
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity:
  x: nan
  y: nan
  z: nan
angular_velocity_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,0.0]
linear_acceleration:
  x: nan
  y: nan
  z: nan
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---
user:~$

In the output above, we are more interested in the orientation, because is what tells us whether the robot is upside down or not.

In order to detect when the robot is upside down, we execute the imu_behaviour.py found on the catkin_ws with the command below:

rosrun example_python_classes imu_behaviour.py

By manually turning the robot upside down, we can see in the output of this script that it correctly detects when that happens. In order to move the robot upside down, on gzweb (the simulation interface), you click on the “Rotate Mode“, then you rotate one of the arcs. In the example below, we rotate the green arc.

Gurdy upside down on ROSDS

Moving the robot to the right orientation automatically

When we detect that the robot is upside down, we have to move its legs, so that the robot can move back to the right position. The ROS topics we have to write to are 6 and can be found with rostopic list | grep command .

user:~$ rostopic list | grep command
/gurdy/head_upperlegM1_joint_position_controller/command
/gurdy/head_upperlegM2_joint_position_controller/command
/gurdy/head_upperlegM3_joint_position_controller/command
/gurdy/upperlegM1_lowerlegM1_joint_position_controller/command
/gurdy/upperlegM2_lowerlegM2_joint_position_controller/command
/gurdy/upperlegM3_lowerlegM3_joint_position_controller/command
user:~$

We have the script called imu_behaviour_2.py which automatically detects when the robot is upside down and publish on the topics listed above to have the robot back to normal position. The content of the file is the following:

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import Imu
from tf.transformations import euler_from_quaternion
from std_msgs.msg import Float64

def extract_rpy_imu_data (imu_msg):

    orientation_quaternion = imu_msg.orientation
    orientation_list = [orientation_quaternion.x,
                        orientation_quaternion.y,
                        orientation_quaternion.z,
                        orientation_quaternion.w]
    roll, pitch, yaw = euler_from_quaternion (orientation_list)
    return roll, pitch, yaw


def detect_upsidedown(roll, pitch, yaw):

    detected_upsidedown = False

    rospy.loginfo("[roll, pitch, yaw]=["+str(roll)+","+str(pitch)+","+str(yaw)+"]")

    # UpRight: 0.011138009176,0.467822324143,2.45108157992

    # UpSideDown: [-3.1415891735,-2.12226602154e-05,2.38423951221]
    # UpSideDown: [3.14141489641,-0.000114323270767,2.38379058991]

    roll_trigger = 3.0

    if abs(roll) > roll_trigger:
        rospy.logwarn("UPASIDEDOWN-ROLL!")
        detected_upsidedown = True

    return detected_upsidedown


def execute_movement_gurdy(movement):

    pub_upperlegM1_joint_position = rospy.Publisher(
            '/gurdy/head_upperlegM1_joint_position_controller/command',
            Float64,
            queue_size=1)
    pub_upperlegM2_joint_position = rospy.Publisher(
        '/gurdy/head_upperlegM2_joint_position_controller/command',
        Float64,
        queue_size=1)
    pub_upperlegM3_joint_position = rospy.Publisher(
        '/gurdy/head_upperlegM3_joint_position_controller/command',
        Float64,
        queue_size=1)
    pub_lowerlegM1_joint_position = rospy.Publisher(
        '/gurdy/upperlegM1_lowerlegM1_joint_position_controller/command',
        Float64,
        queue_size=1)
    pub_lowerlegM2_joint_position = rospy.Publisher(
        '/gurdy/upperlegM2_lowerlegM2_joint_position_controller/command',
        Float64,
        queue_size=1)
    pub_lowerlegM3_joint_position = rospy.Publisher(
        '/gurdy/upperlegM3_lowerlegM3_joint_position_controller/command',
        Float64,
        queue_size=1)


    if movement == "flip":
        rospy.logwarn("FLIP MOVEMENT")
        upperlegM1_angle = -0.7
        upperlegM2_angle = -0.7
        upperlegM3_angle = 0.7
        lowerlegM1_angle = 0.0
        lowerlegM2_angle = 0.0
        lowerlegM3_angle = 0.0
    else:
        rospy.logwarn("basic_stance MOVEMENT")
        upperlegM1_angle = -1.55
        upperlegM2_angle = -1.55
        upperlegM3_angle = -1.55
        lowerlegM1_angle = -2.55
        lowerlegM2_angle = -2.55
        lowerlegM3_angle = -2.55

    upperlegM1 = Float64()
    upperlegM1.data = upperlegM1_angle
    upperlegM2 = Float64()
    upperlegM2.data = upperlegM2_angle
    upperlegM3 = Float64()
    upperlegM3.data = upperlegM3_angle

    lowerlegM1 = Float64()
    lowerlegM1.data = lowerlegM1_angle
    lowerlegM2 = Float64()
    lowerlegM2.data = lowerlegM2_angle
    lowerlegM3 = Float64()
    lowerlegM3.data = lowerlegM3_angle

    pub_upperlegM1_joint_position.publish(upperlegM1)
    pub_upperlegM2_joint_position.publish(upperlegM2)
    pub_upperlegM3_joint_position.publish(upperlegM3)

    pub_lowerlegM1_joint_position.publish(lowerlegM1)
    pub_lowerlegM2_joint_position.publish(lowerlegM2)
    pub_lowerlegM3_joint_position.publish(lowerlegM3)

def get_imu_data(msg):
    roll, pitch, yaw = extract_rpy_imu_data(msg)
    detected_upsidedown = detect_upsidedown(roll, pitch, yaw)
    # Do something based on detect_upsidedown
    if detected_upsidedown:
        movement = "flip"
    else:
        movement = "basic_stance"
    execute_movement_gurdy(movement)




# We Start Here
rospy.init_node('imu')

sub = rospy.Subscriber ('/gurdy/imu/data', Imu, get_imu_data)

rospy.spin()

The magic happens on the function get_imu_data shown above. Based on the position of the robot it decides which movement is going to be executed. We can execute the script with the command below:

rosrun example_python_classes imu_behaviour_2.py

With the command above running, if we now flip the robot around, it will automatically recover its position.

Optimizing the code

Although the imu_behaviour_2.py does the job it is proposed, everything is in a single file and publishers are created every time the execute_movement_gurdy function is called by the get_imu_data one. In addition to that, if we are working on a big project, it is better to have some people working on detecting when the robot is upside down, other people detecting on flipping the robot, so on and so forth.

To make this possible, it is better to have different python files, each one with its different class. We do that by creating the

rosrun example_python_classes imu_behaviour_4.py

So, that is the post of today. We hope you guys liked it. If so, please share this post with your friends.

Video with the explanation of this post

Remember that we also have a live version of this post, which can be found on the video below:

If you liked the video and the post, please leave your thoughts on the comments section of the video. You can also subscribe to our channel on YouTube and press the bell for a new video every day.

Post edited by Miguel, Ricardo, and Ruben.

[ROS Q&A] 191 – How to load a pre-built map into ROS for the Navigation Stack

About

Learn how to load a pre-built map into ROS for the Navigation Stack.

You’ll learn:

• Create a map_server launch for loading a map
• Load the Map into the Navigation Stack

ROSject of the video

• ROSject

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 🙂

[ROS Q&A] 190 – How to subscribe with a C++ subscriber to a custom msg array published with a Python publisher

In the post, you will learn how to create a C++ subscriber to a custom msg array published with Python publisher. This answers the following question on the ROS Answers forum: https://answers.ros.org/question/324325/how-to-use-c-subscribe-to-a-custom-msg-array-published-by-python/.

What you will learn

1. How to create a custom message
2. How to create a ROS Publisher with Python
3. How to create a ROS Subscriber with C++

On your marks…get set…let’s go!

Related Resources

• ROS Development Studio (ROSDS)
• Robot Ignite Academy
• ROS Basics Course (Python)
• ROS Basics Course (C++)

Step 0: Get your development environment ready

Either of the following will do:

1. Use the ROS Development Studio (ROSDS), an online platform for developing for ROS within a PC browser. Easy-peasy. I’m using this option for this post. 
   1. Once you log in, click on the New ROSject button to create a project that will host your code. Then Click on Open ROSject to launch the development/simulation environment.
   2. To open a “terminal” on ROSDS, pick the Shell app from the Tools menu.
   3. You can find the IDE app on the Tools menu.
2. You have ROS installed on a local PC. Okay, skip to Step 2.

Next step!

Step 1: Create the custom messages

Err, not so fast! We need to create a package first. So, open a terminate, browse to your catkin workspace source directory and create the package and some of the files we will need for the custom messages. Note that when creating the package, we added both rospy and roscpp because we will be working with C++ and Python source codes.

cd ~/catkin_ws/src
catkin_create_pkg cpp_python roscpp rospy
cd cpp_python
mkdir -p msg
touch InfoData.msg
touch InfoDataArray.msg

Now open the IDE and locate the open both message files in the msg subdirectory of the package. In InfoData.msg, paste in the following:

int32 num
string color
string name

Put this in InfoDataArray.msg:

InfoData[] infos

Add the following lines to your CMakeLists.txt and package.xml in order to recognize the custom messages:

CMakeLists.txt:

1. Find the existing add_message_files directive, uncomment it, and change it to the following:

add_message_files(
   FILES
   InfoData.msg
   InfoDataArray.msg
 )

2.  Find the generate_messages directive and uncomment it:

generate_messages(
   DEPENDENCIES
   std_msgs  # Or other packages containing msgs
 )

3. Find the catkin_package directive and uncomment the CATKIN_DEPENDS line:

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES cpp_python
  CATKIN_DEPENDS roscpp rospy
#  DEPENDS system_lib
)

Of course, please save the file!

package.xml:

Add the following lines required to generate the custom messages:

<build_depend>message_generation</build_depend>
<build_export_depend>message_generation</build_export_depend>
<exec_depend>message_runtime</exec_depend>

Save!

Compile the messages:

# make sure you run this from the root of your catkin workspace (e.g catkin_ws)
catkin_make

Source the workspace:

# make sure you run this from the root of your catkin workspace (e.g catkin_ws)
source devel/setup.bash

Step 2: Create the Python publisher node

Create a python file in the src folder and make it executable

roscd cpp_python
cd src
touch infodata_publisher.py
chmod +x infodata_publisher.py

Using the IDE, paste in the following code

#! /usr/bin/env python

import rospy # import the ros Python api
from cpp_python.msg import InfoData, InfoDataArray # import the custom messages

rospy.init_node('infodata_publisher_node') # create a node to run the publisher
pub = rospy.Publisher('/infodata', InfoDataArray, queue_size=1) # create the publisher
rate = rospy.Rate(1) # create a rate (per second) for publishing
infodata = InfoData() # create the infodata message
infodata_array = InfoDataArray() # create the infodata array
# set the variables of the info data message
infodata.num = 5
infodata.color = 'red'
infodata.name = 'data1'

# push several infodata messages into the info data array
for x in range(10):
    infodata_array.infos.append(infodata)

# continously publish the infodata array at the rate specified
# until someone stops the node
while not rospy.is_shutdown():
  pub.publish(infodata_array.infos)
  rate.sleep()

Step 3: Create the C++ subscriber node

Create a C++ file in the src folder

roscd cpp_python
cd src
touch infodata_subscriber.cpp

Locate the file in the IDE and paste in the following code:

// Import the infodata messaages
#include <cpp_python/InfoData.h>
#include <cpp_python/InfoDataArray.h>
#include <ros/ros.h> // import the ros C++ api

// the function that takes the message from the subscriber and does
// something with it
void infodataCallback(const cpp_python::InfoDataArray::ConstPtr &msg) {
  ROS_INFO("%d", msg->infos[0].num);
  ROS_INFO("%s", msg->infos[0].color.c_str());
  ROS_INFO("%s", msg->infos[0].name.c_str());
}

int main(int argc, char **argv) {
  // create a node for running the subscriber
  ros::init(argc, argv, "infodata_subscriber_node");
  ros::NodeHandle nh;

  // create the subscriber
  ros::Subscriber sub = nh.subscribe("/infodata", 1000, infodataCallback);

  // Run the subscriber until someone stops the program with Ctrl + C
  ros::spin();

  return 0;
}

Add the following lines to CMakeLists.txt, under the Build section, so the C++ can be compiled:

add_executable(infodata_subscriber src/infodata_subscriber.cpp)
add_dependencies(infodata_subscriber ${cpp_python_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(infodata_subscriber
   ${catkin_LIBRARIES}
 )

Compile the C++ node:

# make sure you run this from the root of your catkin workspace (e.g catkin_ws)
catkin_make

Step 4: Run the Python publisher node

# make sure you run this from the root of your catkin workspace (e.g catkin_ws)
source devel/setup.bash
nohup roscore &
rosrun cpp_python infodata_publisher.py

Nothing is happening? Nah, wait until we launch the subscriber! So, then, let’s do it!

Step 5: Run the C++ subscriber node

Open another terminal and run:

user:~$ cd catkin_ws/
user:~/catkin_ws$ source devel/setup.bash
user:~/catkin_ws$ rosrun cpp_python infodata_subscriber
[ INFO] [1583243129.975478016]: 5
[ INFO] [1583243129.975588039]: red
[ INFO] [1583243129.975620409]: data1
[ INFO] [1583243130.975436776]: 5
[ INFO] [1583243130.975521758]: red
[ INFO] [1583243130.975555258]: data1
[ INFO] [1583243131.975349645]: 5
# ...

So that was it! That’s how to create a C++ subscriber for a Python publisher node.

Extra: Video of this post

We made a video showing how we solved this challenge. If you prefer “sights and sounds” to “black and white”, here you go:

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.

Edited by Bayode Aderinola

Want to play with the complete code right away? You can get a copy of the ROSject at the link below (requires an account on ROSDS).

• ROSject

[ROS Q&A] 189 – How to create a custom action message

About

Learn how to create and compile a custom action message for ROS Actions from zero.

You’ll learn

• How to define the message 
• How to modify the CMakeLists.txt and package.xml files
• How to compile and import the message for ROS Actions

Step 1

• Head to ROS Development Studio and create a new project.
• Provide a suitable project name and some useful description. (We have named the project how_to_make_custom_ros_action)
• Load/Start the project (this will take a few seconds).
• Let’s create a new catkin package in our catkin workspace. Open Tools > Shell and run the following commands

$ cd catkin_ws/src
$ catkin_create_pkg turtlebot3_console_controller actionlib_msgs std_msgs

• We’ll create an action directory to put the action file. We will create an action file named Motor.action inside the action directory

$ cd turtlebot3_console_controller
$ mkdir action
$ touch Motor.action

• Use Tools > IDE to browse the directory and file we just created. At this point we should have the following directory structure

  .
  ├── ai_ws
  ├── catkin_ws
  │   ├── build
  │   ├── devel
  │   └── src
  │       ├── CMakeLists.txt
  │       └── turtlebot3_console_controller
  │           ├── action
  │           │   └── Motor.action
  │           ├── CMakeLists.txt
  │           └── package.xml
  ├── how_to_make_custom_ros_action.zip
  ├── notebook_ws
  │   ├── default.ipynb
  │   └── images
  └── simulation_ws
    ├── build
    ├── devel
    └── src
  

# This is an action definition file, which has three parts: the goal
# the result, and the feedback.
# Part 1: the goal.
#
# The angle in degree the robot to turn, sent by client main
float64 angle
---
# Part 2: the result, sent by action server unpon completion
#
# How much time used
duration time_elapsed
# How many updates thrown in total
uint32 updates_n
---
# Part 3: the feedback,to be sent periodically by server
#
# The amount of time elapsed from the start
duration time_elapsed

from turtlebot3_console_controller.msg import MotorGoal, MotorResult, MotorFeedback 

print("OK, it worked!!!")

Related courses

• Courses for ROS Beginners(Python)
• Courses for ROS Beginners(C++)