HeRo

HeRo

This project contributes to an open source ROS-based framework for swarm robotics. We propose an low cost, high availability swarm system that could be printed and assembled multiple times without special knowledge or hardware skills.

Main Page: https://verlab.github.io/hero_common/ros/

GitHub: https://github.com/verlab/hero_common/

HeRo ROS Tutorial

In this tutorial, we will create a new script to navigate the HeRo. To follow along, you must have ROS installed; for this tutorial, we will be using ROS Noetic.

The HeRo main page already has excellent tutorials for setting up the HeRo. You can follow the steps here:

• HeRo Installation Guide
• HeRo ROS Setup Guide

Alternatively, you can follow this tutorial, which will focus primarily on the ROS setup and development aspects. For the purposes of this tutorial, we will configure HeRo as we would with any other ROS package

Set up ROS Workspace

Create a new ROS workspace. Notice you can use catkin build or catkin_make commands, choose one.

mkdir -p ~ ws_hero/src
cd ws_hero
catkin build

Download the package to ws_hero/src and build it from source. This is common to do for installing ROS packages.

cd ~/ws_hero/src
git clone https://github.com/verlab/hero_common.git
cd ..
rosdep install --from-paths src/hero_common --ignore-src -r -y
catkin build
source devel/setup.bash

If you encounter issues in later steps, you may need to select the repository branch of your ROS distribution, see HeRo Installation Guide. You also may need to install the NVIDIA Container Toolkit. To test if the package is correctly installed, try launch the bringup:

roslaunch hero_bringup hero_bringup.launch

Because we haven’t configured the HeRo robot yet, it will currently search for TCP connections without successfully connecting. However, the launch process itself should still succeed. If everything was set up correctly, you should see the following output in the terminal:

You can exit the terminal.

Connect with HeRo

The HeRo ROS Setup Guide on the main HeRo page provides detailed instructions on connecting your computer to the robot. We summarize the main steps below for quick reference.

• Create a network Hotspot on your PC
  • Set SSID and Password as desired, by default HeRo should connect in a network with SSID = rezeck, password = s3cr3tp4ss.
  • If necessary, set these other Hotspot informations:
    • mode: access point;
    • band: automatic;
    • wifi-security: WPA/WPA2 Personal - store password for all users (not encrypted);
    • ipv4 shared to other computers;
    • ipv6 automatic.

• Configure HeRo network
  • Turn on HeRo in configuration mode: cover all sensors and flip the switch to turn it on. HeRo will blink in purple;
  • Connect any device to the HeRo wi-fi network, password should be s3cr3tp4ss;
  • Open in a browser 192.168.4.1 to open the HeRo configuration page;
  • Set the SSID and Password that you gave to your Hotspot, HeRo will try to connect to this network;
  • Save and turn-off HeRo.

Now turn it on the HeRo, it should connect automatically in the network. The robot will blink blue for 2 seconds if it is connected to the network, otherwise it will keep blink in red.

HeRo Teleoperation

Once the HeRo is connected to the network, you can launch the bringup and check the list of topics to ensure everything is configured correctly.

source devel/setup.bash
roslaunch hero_bringup hero_bringup.launch
rostopic list

If the topics appear as expected, you can proceed to launch teleoperation mode and begin controlling HeRo directly. Make sure to adjust the id to match your HeRo’s ID.

source devel/setup.bash
roslaunch hero_bringup hero_teleop.launch id:=0

Create ROS Package for HeRo

In this tutorial, we will create a package with one node. We will replicate the random walk, HeRo will randomly move and deviate from obstacles, already implemented in the HeRo package. The ROS main page also provides a step-by-step guide in how to create packages Create ROS Package.

You may want to create a new package or node for HeRo. To create a node quickly, navigate to /ws_hero/src/hero_common/hero_examples/scripts/, copy an example script similar to what you want to achieve, modify it as needed, and save it with a new name. Once saved, you can call your node, and it should function properly. To keep your custom work separate from the original package, consider creating an entirely new package for your nodes. This approach keeps your workspace organized and makes it easier to manage changes independently of the original package.

Create a new package

cd src
catkin_create_pkg hero_tutorial std_msgs rospy roscpp
cd ..
catkin_build

Alternatively you can also create a new package inside the hero_common. Those commands will create a new src folder inside your new tutorial_hero, you can create your new scripts inside this folder or create a new folder called scripts.

cd src/hero_tutorial
mkdir scripts
cd scripts

Now, create a python file to start writing our node, we will call it tutorial.py, you need to do chmod +x tutorial.py. Use rostopic list for a ideia to which topics you want to change. In our case, we want to move the robot, therefore, we will use the vellocity_controller/cmd_vel topic to send velocity commands to the robot. First, lets import rospy, create our main function and initialize our new node hero_walk.

#!/usr/bin/env python
import rospy

if __name__ == '__main__':
    rospy.init_node('hero_walk')

Now, we create a new function with a publisher, to publish constantly a velocity of 0.01 in the x direction. We want to publish in the /hero_i/vellocity_controller/cmd_vel topic, changing "i" with the number of your HeRo. To publish in a topic, it is needed to create the type of message expected, to see which message type the topic expects:

rostopic info /hero_i/velocity_controller/cmd_vel

And to see how this type of message is structured:

rosmsg show geometry_msgs/Twist

Now we can build our code, importing the correct message type and we will publish one time every second, or 1 Hz.

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist

def sendVel():
    pub = rospy.Publisher('hero_3/velocity_controller/cmd_vel', Twist, queue_size=1)
    msg = Twist()
    rate = rospy.Rate(1)

    while not rospy.is_shutdown():
        msg.linear.x = 0.01
        msg.angular.z = 0.0  
        pub.publish(msg)
        rate.sleep()

if __name__ == '__main__':
    rospy.init_node('hero_walk')
    sendVel()
    

Now, build and run the node, be careful as the HeRo will walk until you stop the node with ctrl+c. Remember to source.

cd ws_hero
catkin build
source devel/setup.bash
rosrun hero_tutorial tutorial.py

Subscribing and Publishing on HeRo

Now, lets get HeRo informations, specifically, the odometry. Lets create a node that subscribes and print HeRo information. The robot sends its pose information in the topic /odom.

The subscriber node needs a callback function to be defined, in which, we get the pose and print it.

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry

def callback(msg):
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    rz = msg.pose.pose.orientation.z
    message = '\nPose X: ' + str(x) + '\nPose Y: ' + str(y) + '\nRotation Z: ' + str(rz)
    rospy.loginfo(rospy.get_caller_id() + "%s", message)

def printOdom():
    rospy.Subscriber('hero_3/odom', Odometry, callback)
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        #rospy.login("Print Pose")
        rate.sleep()

if __name__ == '__main__':
    rospy.init_node('hero_sub_odometry')
    printOdom()
    

Controlling HeRo

It is also possible to implement subscribers and publishers within the same code, use the subscriber data to publish messages according with the data, we provide a control example with HeRo. The full code is below, we will break and explain step-by-step.

#!/usr/bin/env python3
import rospy
import numpy as np
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist

def normalize_angle(angle):
    return np.mod(angle+np.pi, 2*np.pi) - np.pi

def euler_from_quaternion(x, y, z, w):
        """
        Convert a quaternion into euler angles (roll, pitch, yaw)
        roll is rotation around x in radians (counterclockwise)
        pitch is rotation around y in radians (counterclockwise)
        yaw is rotation around z in radians (counterclockwise)
        """
        t0 = +2.0 * (w * x + y * z)
        t1 = +1.0 - 2.0 * (x * x + y * y)
        roll_x = np.arctan2(t0, t1)
     
        t2 = +2.0 * (w * y - z * x)
        t2 = +1.0 if t2 > +1.0 else t2
        t2 = -1.0 if t2 < -1.0 else t2
        pitch_y = np.arcsin(t2)
     
        t3 = +2.0 * (w * z + x * y)
        t4 = +1.0 - 2.0 * (y * y + z * z)
        yaw_z = np.arctan2(t3, t4)
     
        return roll_x, pitch_y, yaw_z # in radians

def control(x,y,x_end,y_end,theta):
    maxv = 0.07
    maxw = 0.1
    kr = 4 / 20
    ka = 8 / 20

    dx = x_end - x
    dy = y_end - y
    rho = np.sqrt(dx**2 + dy**2)
    alpha = normalize_angle(-theta + np.arctan2(dy,dx))
    
    if abs(alpha) > np.pi/2:
         kr = -kr
         alpha = normalize_angle(alpha - np.pi)

    v = kr*rho
    w = ka*alpha
    v = max(min(v, maxv), -maxv)
    w = max(min(w, maxw), -maxw)

    return v,w

class ControlHero(object):
    def __init__(self):
          self.cmd_vel = Twist()
          self.odom = Odometry()
          self.x_end = 0.15
          self.y_end = 0.00

    def callback(self, data):
        self.odom = data

    def run(self):
        # Initiate node and create publish and subscriber
        rospy.init_node('hero_control')
        rospy.Subscriber('hero_3/odom', Odometry, self.callback)
        self.pub = rospy.Publisher('hero_3/velocity_controller/cmd_vel', Twist, queue_size=1)

        # Preparing message
        msg = Twist()

        # Define rate
        rate = rospy.Rate(5)           

        while not rospy.is_shutdown():
            # Getting information
            x = self.odom.pose.pose.position.x
            y = self.odom.pose.pose.position.y
            w = self.odom.pose.pose.orientation.w
            rx = self.odom.pose.pose.orientation.x
            ry = self.odom.pose.pose.orientation.y
            rz = self.odom.pose.pose.orientation.z

            # Computing control outputs
            ex, ey, ez = euler_from_quaternion(rx,ry,rz,w)
            vx, vw = control(x,y,self.x_end,self.y_end,ez)

            # Preparing message
            msg.linear.x = vx
            msg.angular.z = vw

            # Stop condition
            if (np.linalg.norm(np.array((x,y)) - np.array((self.x_end,self.y_end)))) < 0.01:
                msg.linear.x = 0
                msg.angular.z = 0
                rospy.loginfo_once("Chegou")           

            # Publish velocity
            self.pub.publish(msg)

            rate.sleep

if __name__ == '__main__':
    obj = ControlHero()
    obj.run()

The function euler_from_quaternion converts quaternions into Euler angles, while control is a position proportional controller that calculates linear and angular velocities. We create a class to enable the use of subscriber data for online computation of control outputs. Alternatively, global variables could be used, but this method is less structured.

In the main function, we simply instantiate an object of the class we defined and call its run function, where the core functionality is executed.

if __name__ == '__main__':
    obj = ControlHero()
    obj.run()

The class is initialized with key variables that are accessible throughout its methods, functioning similarly to global variables but within the scope of the class. In our case, these variables include the messages we want to send and receive, as well as the desired final position (x,y) for HeRo.

    def __init__(self):
          self.cmd_vel = Twist()
          self.odom = Odometry()
          self.x_end = 0.15
          self.y_end = 0.00

The run function first initializes the node, sets up a publisher to send angular and linear velocities to HeRo, and creates a subscriber to retrieve pose information from HeRo.

    def run(self):
        # Initiate node and create publish and subscriber
        rospy.init_node('hero_control')
        rospy.Subscriber('hero_3/odom', Odometry, self.callback)
        self.pub = rospy.Publisher('hero_3/velocity_controller/cmd_vel', Twist, queue_size=1)

Since we need HeRo's pose for control, the subscriber callback function is used to retrieve the pose data and store it in a class variable for easy access during computations.

def callback(self, data):
        self.odom = data

Continuing the run function, we initiate the variable with the correct message type to send the velocities and set up the rate in which we will send those velocities, in our case, 5 times a second (5 Hz) will be enough.

# Preparing message
msg = Twist()

# Define rate
rate = rospy.Rate(5)   

Next, we extract information from the self.odom variable, which the subscriber updates with HeRo's pose data in real time, and store it in local variables for processing. Using these variables, we compute the velocities required to control HeRo and prepare the message to send to the publisher.

while not rospy.is_shutdown():
    # Getting information
    x = self.odom.pose.pose.position.x
    y = self.odom.pose.pose.position.y
    w = self.odom.pose.pose.orientation.w
    rx = self.odom.pose.pose.orientation.x
    ry = self.odom.pose.pose.orientation.y
    rz = self.odom.pose.pose.orientation.z

    # Computing control outputs
    ex, ey, ez = euler_from_quaternion(rx,ry,rz,w)
    vx, vw = control(x,y,self.x_end,self.y_end,ez)

    # Preparing message
    msg.linear.x = vx
    msg.angular.z = vw

Finally, after defining our stop condition as reaching within 0.01 units of the goal, we publish the computed velocities in message format.

    # Stop condition
    if (np.linalg.norm(np.array((x,y)) - np.array((self.x_end,self.y_end)))) < 0.01:
        msg.linear.x = 0
        msg.angular.z = 0
        rospy.loginfo_once("Chegou")           

    # Publish velocity
    self.pub.publish(msg)

    rate.sleep()

Now, set the desired (x,y) coordinates for HeRo to achieve and run the node. Remember to chmod +x tutorial_control.py.

source devel/setup.bash
rosrun hero_tutorial tutorial_control.py

Keep in mind that HeRo's (x,y)=(0,0) corresponds to the position where it was powered on. It's important to note that HeRo lacks precise pose estimation by itself. Ideally, an external camera setup would be used to provide more accurate position feedback. As a result, you might observe HeRo stopping at a location that differs from the expected destination. However, according to HeRo's internal estimation, it may have reached the correct position. To verify HeRo's perception of its position, you can either use the echo command on the odometry topic or run the subscriber node we created earlier.