HeRo
Contents
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 and utilize its sensors to avoid obstacles. To follow along, you must have ROS installed; for this tutorial, I will be using ROS Noetic.
The HeRo main page already has excellent tutorials for setting up the HeRo. You can follow the steps here:
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/position_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 normalizeAngle(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 = normalizeAngle(-theta + np.arctan2(dy,dx))
if abs(alpha) > np.pi/2:
kr = -kr
alpha = normalizeAngle(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()
