VeRLab Wiki - User contributions [en]

HeRo

2024-11-22T17:01:56Z

Caioconti: /* HeRo ROS Tutorial */

== 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.

[[File:Hero.png|300px]]

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:
*[https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]
*[https://verlab.github.io/hero_common/ros/ 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 <code>catkin build</code> or <code>catkin_make</code> commands, choose one.

<pre>
mkdir -p ~ ws_hero/src
cd ws_hero
catkin build
</pre>

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

<pre>
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
</pre>

If you encounter issues in later steps, you may need to select the repository branch of your ROS distribution, see [https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]. You also may need to install [https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html the NVIDIA Container Toolkit]. To test if the package is correctly installed, try launch the bringup:

<pre>
roslaunch hero_bringup hero_bringup.launch
</pre>

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:

[[File:first_roslaunch.png]]

You can exit the terminal.

===Connect with HeRo===
The [https://verlab.github.io/hero_common/ros/ 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.

<pre>
source devel/setup.bash
roslaunch hero_bringup hero_bringup.launch
rostopic list
</pre>

If the topics appear as expected, you can proceed to launch teleoperation mode and begin controlling HeRo directly. Make sure to adjust the <code>id</code> to match your HeRo’s ID.
<pre>
source devel/setup.bash
roslaunch hero_bringup hero_teleop.launch id:=0
</pre>

=== 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 [http://wiki.ros.org/ROS/Tutorials/CreatingPackage Create ROS Package].

You may want to create a new package or node for HeRo. To create a node quickly, navigate to <code>/ws_hero/src/hero_common/hero_examples/scripts/</code>, 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
<pre>
cd src
catkin_create_pkg hero_tutorial std_msgs rospy roscpp
cd ..
catkin_build
</pre>

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

<pre>
cd src/hero_tutorial
mkdir scripts
cd scripts
</pre>

Now, create a python file to start writing our node, we will call it tutorial.py, you need to do <code>chmod +x tutorial.py</code>. 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 <code>vellocity_controller/cmd_vel</code> topic to send velocity commands to the robot. First, lets import rospy, create our main function and initialize our new node <code>hero_walk</code>.

<pre>
#!/usr/bin/env python
import rospy

if __name__ == '__main__':
rospy.init_node('hero_walk')
</pre>

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 <code>/hero_i/vellocity_controller/cmd_vel</code> 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:

<code>rostopic info /hero_i/velocity_controller/cmd_vel</code>

And to see how this type of message is structured:

<code>rosmsg show geometry_msgs/Twist</code>

Now we can build our code, importing the correct message type and we will publish one time every second, or 1 Hz.
<pre>
#!/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()

</pre>

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

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

==== 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 <code>/odom</code>.

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

<pre>
#!/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()

</pre>

==== 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.

<pre>
#!/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()
</pre>

The function <code>euler_from_quaternion</code> converts quaternions into Euler angles, while <code>control</code> 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.

<pre>
if __name__ == '__main__':
obj = ControlHero()
obj.run()
</pre>

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.
<pre>
def __init__(self):
self.cmd_vel = Twist()
self.odom = Odometry()
self.x_end = 0.15
self.y_end = 0.00
</pre>

The <code>run</code> 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.

<pre>
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)
</pre>

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.

<pre>
def callback(self, data):
self.odom = data
</pre>

Continuing the <code>run</code> 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.

<pre>
# Preparing message
msg = Twist()

# Define rate
rate = rospy.Rate(5)
</pre>

Next, we extract information from the <code>self.odom</code> 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.

<pre>
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
</pre>

Finally, after defining our stop condition as reaching within 0.01 units of the goal, we publish the computed velocities in message format.
<pre>
# 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()
</pre>

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

<pre>
source devel/setup.bash
rosrun hero_tutorial tutorial_control.py
</pre>

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 <code>echo</code> command on the odometry topic or run the subscriber node we created earlier.

HeRo

2024-11-22T03:02:37Z

Caioconti: /* Create ROS Package for 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.

[[File:Hero.png|300px]]

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:
*[https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]
*[https://verlab.github.io/hero_common/ros/ 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 <code>catkin build</code> or <code>catkin_make</code> commands, choose one.

<pre>
mkdir -p ~ ws_hero/src
cd ws_hero
catkin build
</pre>

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

<pre>
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
</pre>

If you encounter issues in later steps, you may need to select the repository branch of your ROS distribution, see [https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]. You also may need to install [https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html the NVIDIA Container Toolkit]. To test if the package is correctly installed, try launch the bringup:

<pre>
roslaunch hero_bringup hero_bringup.launch
</pre>

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:

[[File:first_roslaunch.png]]

You can exit the terminal.

===Connect with HeRo===
The [https://verlab.github.io/hero_common/ros/ 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.

<pre>
source devel/setup.bash
roslaunch hero_bringup hero_bringup.launch
rostopic list
</pre>

If the topics appear as expected, you can proceed to launch teleoperation mode and begin controlling HeRo directly. Make sure to adjust the <code>id</code> to match your HeRo’s ID.
<pre>
source devel/setup.bash
roslaunch hero_bringup hero_teleop.launch id:=0
</pre>

=== 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 [http://wiki.ros.org/ROS/Tutorials/CreatingPackage Create ROS Package].

You may want to create a new package or node for HeRo. To create a node quickly, navigate to <code>/ws_hero/src/hero_common/hero_examples/scripts/</code>, 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
<pre>
cd src
catkin_create_pkg hero_tutorial std_msgs rospy roscpp
cd ..
catkin_build
</pre>

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

<pre>
cd src/hero_tutorial
mkdir scripts
cd scripts
</pre>

Now, create a python file to start writing our node, we will call it tutorial.py, you need to do <code>chmod +x tutorial.py</code>. 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 <code>vellocity_controller/cmd_vel</code> topic to send velocity commands to the robot. First, lets import rospy, create our main function and initialize our new node <code>hero_walk</code>.

<pre>
#!/usr/bin/env python
import rospy

if __name__ == '__main__':
rospy.init_node('hero_walk')
</pre>

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 <code>/hero_i/vellocity_controller/cmd_vel</code> 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:

<code>rostopic info /hero_i/velocity_controller/cmd_vel</code>

And to see how this type of message is structured:

<code>rosmsg show geometry_msgs/Twist</code>

Now we can build our code, importing the correct message type and we will publish one time every second, or 1 Hz.
<pre>
#!/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()

</pre>

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

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

==== 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 <code>/odom</code>.

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

<pre>
#!/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()

</pre>

==== 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.

<pre>
#!/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()
</pre>

The function <code>euler_from_quaternion</code> converts quaternions into Euler angles, while <code>control</code> 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.

<pre>
if __name__ == '__main__':
obj = ControlHero()
obj.run()
</pre>

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.
<pre>
def __init__(self):
self.cmd_vel = Twist()
self.odom = Odometry()
self.x_end = 0.15
self.y_end = 0.00
</pre>

The <code>run</code> 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.

<pre>
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)
</pre>

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.

<pre>
def callback(self, data):
self.odom = data
</pre>

Continuing the <code>run</code> 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.

<pre>
# Preparing message
msg = Twist()

# Define rate
rate = rospy.Rate(5)
</pre>

Next, we extract information from the <code>self.odom</code> 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.

<pre>
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
</pre>

Finally, after defining our stop condition as reaching within 0.01 units of the goal, we publish the computed velocities in message format.
<pre>
# 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()
</pre>

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

<pre>
source devel/setup.bash
rosrun hero_tutorial tutorial_control.py
</pre>

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 <code>echo</code> command on the odometry topic or run the subscriber node we created earlier.

HeRo

2024-11-21T23:13:19Z

Caioconti: /* Controlling 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.

[[File:Hero.png|300px]]

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:
*[https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]
*[https://verlab.github.io/hero_common/ros/ 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 <code>catkin build</code> or <code>catkin_make</code> commands, choose one.

<pre>
mkdir -p ~ ws_hero/src
cd ws_hero
catkin build
</pre>

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

<pre>
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
</pre>

If you encounter issues in later steps, you may need to select the repository branch of your ROS distribution, see [https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]. You also may need to install [https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html the NVIDIA Container Toolkit]. To test if the package is correctly installed, try launch the bringup:

<pre>
roslaunch hero_bringup hero_bringup.launch
</pre>

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:

[[File:first_roslaunch.png]]

You can exit the terminal.

===Connect with HeRo===
The [https://verlab.github.io/hero_common/ros/ 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.

<pre>
source devel/setup.bash
roslaunch hero_bringup hero_bringup.launch
rostopic list
</pre>

If the topics appear as expected, you can proceed to launch teleoperation mode and begin controlling HeRo directly. Make sure to adjust the <code>id</code> to match your HeRo’s ID.
<pre>
source devel/setup.bash
roslaunch hero_bringup hero_teleop.launch id:=0
</pre>

=== 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 [http://wiki.ros.org/ROS/Tutorials/CreatingPackage Create ROS Package].

You may want to create a new package or node for HeRo. To create a node quickly, navigate to <code>/ws_hero/src/hero_common/hero_examples/scripts/</code>, 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
<pre>
cd src
catkin_create_pkg hero_tutorial std_msgs rospy roscpp
cd ..
catkin_build
</pre>

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

<pre>
cd src/hero_tutorial
mkdir scripts
cd scripts
</pre>

Now, create a python file to start writing our node, we will call it tutorial.py, you need to do <code>chmod +x tutorial.py</code>. 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 <code>vellocity_controller/cmd_vel</code> topic to send velocity commands to the robot. First, lets import rospy, create our main function and initialize our new node <code>hero_walk</code>.

<pre>
#!/usr/bin/env python
import rospy

if __name__ == '__main__':
rospy.init_node('hero_walk')
</pre>

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 <code>/hero_i/vellocity_controller/cmd_vel</code> 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:

<code>rostopic info /hero_i/position_controller/cmd_vel</code>

And to see how this type of message is structured:

<code>rosmsg show geometry_msgs/Twist</code>

Now we can build our code, importing the correct message type and we will publish one time every second, or 1 Hz.
<pre>
#!/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()

</pre>

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

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

==== 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 <code>/odom</code>.

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

<pre>
#!/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()

</pre>

==== 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.

<pre>
#!/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()
</pre>

The function <code>euler_from_quaternion</code> converts quaternions into Euler angles, while <code>control</code> 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.

<pre>
if __name__ == '__main__':
obj = ControlHero()
obj.run()
</pre>

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.
<pre>
def __init__(self):
self.cmd_vel = Twist()
self.odom = Odometry()
self.x_end = 0.15
self.y_end = 0.00
</pre>

The <code>run</code> 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.

<pre>
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)
</pre>

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.

<pre>
def callback(self, data):
self.odom = data
</pre>

Continuing the <code>run</code> 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.

<pre>
# Preparing message
msg = Twist()

# Define rate
rate = rospy.Rate(5)
</pre>

Next, we extract information from the <code>self.odom</code> 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.

<pre>
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
</pre>

Finally, after defining our stop condition as reaching within 0.01 units of the goal, we publish the computed velocities in message format.
<pre>
# 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()
</pre>

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

<pre>
source devel/setup.bash
rosrun hero_tutorial tutorial_control.py
</pre>

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 <code>echo</code> command on the odometry topic or run the subscriber node we created earlier.

HeRo

2024-11-21T23:11:38Z

Caioconti: /* Controlling 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.

[[File:Hero.png|300px]]

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:
*[https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]
*[https://verlab.github.io/hero_common/ros/ 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 <code>catkin build</code> or <code>catkin_make</code> commands, choose one.

<pre>
mkdir -p ~ ws_hero/src
cd ws_hero
catkin build
</pre>

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

<pre>
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
</pre>

If you encounter issues in later steps, you may need to select the repository branch of your ROS distribution, see [https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]. You also may need to install [https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html the NVIDIA Container Toolkit]. To test if the package is correctly installed, try launch the bringup:

<pre>
roslaunch hero_bringup hero_bringup.launch
</pre>

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:

[[File:first_roslaunch.png]]

You can exit the terminal.

===Connect with HeRo===
The [https://verlab.github.io/hero_common/ros/ 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.

<pre>
source devel/setup.bash
roslaunch hero_bringup hero_bringup.launch
rostopic list
</pre>

If the topics appear as expected, you can proceed to launch teleoperation mode and begin controlling HeRo directly. Make sure to adjust the <code>id</code> to match your HeRo’s ID.
<pre>
source devel/setup.bash
roslaunch hero_bringup hero_teleop.launch id:=0
</pre>

=== 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 [http://wiki.ros.org/ROS/Tutorials/CreatingPackage Create ROS Package].

You may want to create a new package or node for HeRo. To create a node quickly, navigate to <code>/ws_hero/src/hero_common/hero_examples/scripts/</code>, 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
<pre>
cd src
catkin_create_pkg hero_tutorial std_msgs rospy roscpp
cd ..
catkin_build
</pre>

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

<pre>
cd src/hero_tutorial
mkdir scripts
cd scripts
</pre>

Now, create a python file to start writing our node, we will call it tutorial.py, you need to do <code>chmod +x tutorial.py</code>. 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 <code>vellocity_controller/cmd_vel</code> topic to send velocity commands to the robot. First, lets import rospy, create our main function and initialize our new node <code>hero_walk</code>.

<pre>
#!/usr/bin/env python
import rospy

if __name__ == '__main__':
rospy.init_node('hero_walk')
</pre>

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 <code>/hero_i/vellocity_controller/cmd_vel</code> 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:

<code>rostopic info /hero_i/position_controller/cmd_vel</code>

And to see how this type of message is structured:

<code>rosmsg show geometry_msgs/Twist</code>

Now we can build our code, importing the correct message type and we will publish one time every second, or 1 Hz.
<pre>
#!/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()

</pre>

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

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

==== 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 <code>/odom</code>.

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

<pre>
#!/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()

</pre>

==== 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.

<pre>
#!/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()
</pre>

The function <code>euler_from_quaternion</code> converts quaternions into Euler angles, while <code>control</code> 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.

<pre>
if __name__ == '__main__':
obj = ControlHero()
obj.run()
</pre>

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.
<pre>
def __init__(self):
self.cmd_vel = Twist()
self.odom = Odometry()
self.x_end = 0.15
self.y_end = 0.00
</pre>

The <code>run</code> 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.

<pre>
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)
</pre>

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.

<pre>
def callback(self, data):
self.odom = data
</pre>

Continuing the <code>run</code> 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.

<pre>
# Preparing message
msg = Twist()

# Define rate
rate = rospy.Rate(5)
</pre>

Next, we extract information from the <code>self.odom</code> 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.

<pre>
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
</pre>

Finally, after defining our stop condition as reaching within 0.01 units of the goal, we publish the computed velocities in message format.
<pre>
# 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()
</pre>

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

<pre>
source devel/setup.bash
rosrun hero_tutorial tutorial_control.py
</pre>

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 use the <code>echo</code> command on the odometry topic.

HeRo

2024-11-21T23:04:15Z

Caioconti: /* Controlling 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.

[[File:Hero.png|300px]]

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:
*[https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]
*[https://verlab.github.io/hero_common/ros/ 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 <code>catkin build</code> or <code>catkin_make</code> commands, choose one.

<pre>
mkdir -p ~ ws_hero/src
cd ws_hero
catkin build
</pre>

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

<pre>
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
</pre>

If you encounter issues in later steps, you may need to select the repository branch of your ROS distribution, see [https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]. You also may need to install [https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html the NVIDIA Container Toolkit]. To test if the package is correctly installed, try launch the bringup:

<pre>
roslaunch hero_bringup hero_bringup.launch
</pre>

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:

[[File:first_roslaunch.png]]

You can exit the terminal.

===Connect with HeRo===
The [https://verlab.github.io/hero_common/ros/ 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.

<pre>
source devel/setup.bash
roslaunch hero_bringup hero_bringup.launch
rostopic list
</pre>

If the topics appear as expected, you can proceed to launch teleoperation mode and begin controlling HeRo directly. Make sure to adjust the <code>id</code> to match your HeRo’s ID.
<pre>
source devel/setup.bash
roslaunch hero_bringup hero_teleop.launch id:=0
</pre>

=== 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 [http://wiki.ros.org/ROS/Tutorials/CreatingPackage Create ROS Package].

You may want to create a new package or node for HeRo. To create a node quickly, navigate to <code>/ws_hero/src/hero_common/hero_examples/scripts/</code>, 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
<pre>
cd src
catkin_create_pkg hero_tutorial std_msgs rospy roscpp
cd ..
catkin_build
</pre>

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

<pre>
cd src/hero_tutorial
mkdir scripts
cd scripts
</pre>

Now, create a python file to start writing our node, we will call it tutorial.py, you need to do <code>chmod +x tutorial.py</code>. 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 <code>vellocity_controller/cmd_vel</code> topic to send velocity commands to the robot. First, lets import rospy, create our main function and initialize our new node <code>hero_walk</code>.

<pre>
#!/usr/bin/env python
import rospy

if __name__ == '__main__':
rospy.init_node('hero_walk')
</pre>

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 <code>/hero_i/vellocity_controller/cmd_vel</code> 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:

<code>rostopic info /hero_i/position_controller/cmd_vel</code>

And to see how this type of message is structured:

<code>rosmsg show geometry_msgs/Twist</code>

Now we can build our code, importing the correct message type and we will publish one time every second, or 1 Hz.
<pre>
#!/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()

</pre>

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

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

==== 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 <code>/odom</code>.

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

<pre>
#!/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()

</pre>

==== 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.

<pre>
#!/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()
</pre>

The function <code>euler_from_quaternion</code> converts quaternions into Euler angles, while <code>control</code> 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.

<pre>
if __name__ == '__main__':
obj = ControlHero()
obj.run()
</pre>

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.
<pre>
def __init__(self):
self.cmd_vel = Twist()
self.odom = Odometry()
self.x_end = 0.15
self.y_end = 0.00
</pre>

The <code>run</code> 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.

<pre>
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)
</pre>

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.

<pre>
def callback(self, data):
self.odom = data
</pre>

Continuing the <code>run</code> 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.

<pre>
# Preparing message
msg = Twist()

# Define rate
rate = rospy.Rate(5)
</pre>

Next, we extract information from the <code>self.odom</code> 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.

<pre>
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
</pre>

Finally, after defining our stop condition as reaching within 0.01 units of the goal, we publish the computed velocities in message format.
<pre>
# 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()
</pre>

HeRo

2024-11-21T22:58:17Z

Caioconti: /* Controlling 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.

[[File:Hero.png|300px]]

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:
*[https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]
*[https://verlab.github.io/hero_common/ros/ 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 <code>catkin build</code> or <code>catkin_make</code> commands, choose one.

<pre>
mkdir -p ~ ws_hero/src
cd ws_hero
catkin build
</pre>

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

<pre>
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
</pre>

If you encounter issues in later steps, you may need to select the repository branch of your ROS distribution, see [https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]. You also may need to install [https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html the NVIDIA Container Toolkit]. To test if the package is correctly installed, try launch the bringup:

<pre>
roslaunch hero_bringup hero_bringup.launch
</pre>

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:

[[File:first_roslaunch.png]]

You can exit the terminal.

===Connect with HeRo===
The [https://verlab.github.io/hero_common/ros/ 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.

<pre>
source devel/setup.bash
roslaunch hero_bringup hero_bringup.launch
rostopic list
</pre>

If the topics appear as expected, you can proceed to launch teleoperation mode and begin controlling HeRo directly. Make sure to adjust the <code>id</code> to match your HeRo’s ID.
<pre>
source devel/setup.bash
roslaunch hero_bringup hero_teleop.launch id:=0
</pre>

=== 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 [http://wiki.ros.org/ROS/Tutorials/CreatingPackage Create ROS Package].

You may want to create a new package or node for HeRo. To create a node quickly, navigate to <code>/ws_hero/src/hero_common/hero_examples/scripts/</code>, 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
<pre>
cd src
catkin_create_pkg hero_tutorial std_msgs rospy roscpp
cd ..
catkin_build
</pre>

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

<pre>
cd src/hero_tutorial
mkdir scripts
cd scripts
</pre>

Now, create a python file to start writing our node, we will call it tutorial.py, you need to do <code>chmod +x tutorial.py</code>. 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 <code>vellocity_controller/cmd_vel</code> topic to send velocity commands to the robot. First, lets import rospy, create our main function and initialize our new node <code>hero_walk</code>.

<pre>
#!/usr/bin/env python
import rospy

if __name__ == '__main__':
rospy.init_node('hero_walk')
</pre>

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 <code>/hero_i/vellocity_controller/cmd_vel</code> 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:

<code>rostopic info /hero_i/position_controller/cmd_vel</code>

And to see how this type of message is structured:

<code>rosmsg show geometry_msgs/Twist</code>

Now we can build our code, importing the correct message type and we will publish one time every second, or 1 Hz.
<pre>
#!/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()

</pre>

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

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

==== 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 <code>/odom</code>.

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

<pre>
#!/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()

</pre>

==== 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.

<pre>
#!/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()
</pre>

The function <code>euler_from_quaternion</code> converts quaternions into Euler angles, while <code>control</code> 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.

<pre>
if __name__ == '__main__':
obj = ControlHero()
obj.run()
</pre>

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.
<pre>
def __init__(self):
self.cmd_vel = Twist()
self.odom = Odometry()
self.x_end = 0.15
self.y_end = 0.00
</pre>

The <code>run</code> 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.

<pre>
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)
</pre>

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.

<pre>
def callback(self, data):
self.odom = data
</pre>

Continuing the <code>run</code> 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.

<pre>
# Preparing message
msg = Twist()

# Define rate
rate = rospy.Rate(5)
</pre>

HeRo

2024-11-21T22:51:00Z

Caioconti: /* Controlling 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.

[[File:Hero.png|300px]]

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:
*[https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]
*[https://verlab.github.io/hero_common/ros/ 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 <code>catkin build</code> or <code>catkin_make</code> commands, choose one.

<pre>
mkdir -p ~ ws_hero/src
cd ws_hero
catkin build
</pre>

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

<pre>
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
</pre>

If you encounter issues in later steps, you may need to select the repository branch of your ROS distribution, see [https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]. You also may need to install [https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html the NVIDIA Container Toolkit]. To test if the package is correctly installed, try launch the bringup:

<pre>
roslaunch hero_bringup hero_bringup.launch
</pre>

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:

[[File:first_roslaunch.png]]

You can exit the terminal.

===Connect with HeRo===
The [https://verlab.github.io/hero_common/ros/ 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.

<pre>
source devel/setup.bash
roslaunch hero_bringup hero_bringup.launch
rostopic list
</pre>

If the topics appear as expected, you can proceed to launch teleoperation mode and begin controlling HeRo directly. Make sure to adjust the <code>id</code> to match your HeRo’s ID.
<pre>
source devel/setup.bash
roslaunch hero_bringup hero_teleop.launch id:=0
</pre>

=== 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 [http://wiki.ros.org/ROS/Tutorials/CreatingPackage Create ROS Package].

You may want to create a new package or node for HeRo. To create a node quickly, navigate to <code>/ws_hero/src/hero_common/hero_examples/scripts/</code>, 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
<pre>
cd src
catkin_create_pkg hero_tutorial std_msgs rospy roscpp
cd ..
catkin_build
</pre>

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

<pre>
cd src/hero_tutorial
mkdir scripts
cd scripts
</pre>

Now, create a python file to start writing our node, we will call it tutorial.py, you need to do <code>chmod +x tutorial.py</code>. 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 <code>vellocity_controller/cmd_vel</code> topic to send velocity commands to the robot. First, lets import rospy, create our main function and initialize our new node <code>hero_walk</code>.

<pre>
#!/usr/bin/env python
import rospy

if __name__ == '__main__':
rospy.init_node('hero_walk')
</pre>

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 <code>/hero_i/vellocity_controller/cmd_vel</code> 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:

<code>rostopic info /hero_i/position_controller/cmd_vel</code>

And to see how this type of message is structured:

<code>rosmsg show geometry_msgs/Twist</code>

Now we can build our code, importing the correct message type and we will publish one time every second, or 1 Hz.
<pre>
#!/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()

</pre>

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

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

==== 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 <code>/odom</code>.

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

<pre>
#!/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()

</pre>

==== 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.

<pre>
#!/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()
</pre>

The function <code>euler_from_quaternion</code> converts quaternions into Euler angles, while <code>control</code> 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.

<pre>
if __name__ == '__main__':
obj = ControlHero()
obj.run()
</pre>

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.
<pre>
def __init__(self):
self.cmd_vel = Twist()
self.odom = Odometry()
self.x_end = 0.15
self.y_end = 0.00
</pre>

The <code>run</code> function intiates the node, publisher and subscriber.

<pre>
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)
</pre>

We want the subscriber to get the HeRo pose, therefore, we simply return its data to a class variable in the subscriber callback.

<pre>
def callback(self, data):
self.odom = data
</pre>

HeRo

2024-11-21T22:47:35Z

Caioconti: /* Controlling 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.

[[File:Hero.png|300px]]

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:
*[https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]
*[https://verlab.github.io/hero_common/ros/ 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 <code>catkin build</code> or <code>catkin_make</code> commands, choose one.

<pre>
mkdir -p ~ ws_hero/src
cd ws_hero
catkin build
</pre>

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

<pre>
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
</pre>

If you encounter issues in later steps, you may need to select the repository branch of your ROS distribution, see [https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]. You also may need to install [https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html the NVIDIA Container Toolkit]. To test if the package is correctly installed, try launch the bringup:

<pre>
roslaunch hero_bringup hero_bringup.launch
</pre>

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:

[[File:first_roslaunch.png]]

You can exit the terminal.

===Connect with HeRo===
The [https://verlab.github.io/hero_common/ros/ 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.

<pre>
source devel/setup.bash
roslaunch hero_bringup hero_bringup.launch
rostopic list
</pre>

If the topics appear as expected, you can proceed to launch teleoperation mode and begin controlling HeRo directly. Make sure to adjust the <code>id</code> to match your HeRo’s ID.
<pre>
source devel/setup.bash
roslaunch hero_bringup hero_teleop.launch id:=0
</pre>

=== 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 [http://wiki.ros.org/ROS/Tutorials/CreatingPackage Create ROS Package].

You may want to create a new package or node for HeRo. To create a node quickly, navigate to <code>/ws_hero/src/hero_common/hero_examples/scripts/</code>, 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
<pre>
cd src
catkin_create_pkg hero_tutorial std_msgs rospy roscpp
cd ..
catkin_build
</pre>

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

<pre>
cd src/hero_tutorial
mkdir scripts
cd scripts
</pre>

Now, create a python file to start writing our node, we will call it tutorial.py, you need to do <code>chmod +x tutorial.py</code>. 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 <code>vellocity_controller/cmd_vel</code> topic to send velocity commands to the robot. First, lets import rospy, create our main function and initialize our new node <code>hero_walk</code>.

<pre>
#!/usr/bin/env python
import rospy

if __name__ == '__main__':
rospy.init_node('hero_walk')
</pre>

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 <code>/hero_i/vellocity_controller/cmd_vel</code> 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:

<code>rostopic info /hero_i/position_controller/cmd_vel</code>

And to see how this type of message is structured:

<code>rosmsg show geometry_msgs/Twist</code>

Now we can build our code, importing the correct message type and we will publish one time every second, or 1 Hz.
<pre>
#!/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()

</pre>

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

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

==== 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 <code>/odom</code>.

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

<pre>
#!/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()

</pre>

==== 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.

<pre>
#!/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()
</pre>

The function <code>euler_from_quaternion</code> converts quaternions into Euler angles, while <code>control</code> 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.

<pre>
if __name__ == '__main__':
obj = ControlHero()
obj.run()
</pre>

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.
<pre>
def __init__(self):
self.cmd_vel = Twist()
self.odom = Odometry()
self.x_end = 0.15
self.y_end = 0.00
</pre>

running the <code>run</run> function, firstly initiates

HeRo

2024-11-21T22:35:39Z

Caioconti: /* Controlling 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.

[[File:Hero.png|300px]]

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:
*[https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]
*[https://verlab.github.io/hero_common/ros/ 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 <code>catkin build</code> or <code>catkin_make</code> commands, choose one.

<pre>
mkdir -p ~ ws_hero/src
cd ws_hero
catkin build
</pre>

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

<pre>
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
</pre>

If you encounter issues in later steps, you may need to select the repository branch of your ROS distribution, see [https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]. You also may need to install [https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html the NVIDIA Container Toolkit]. To test if the package is correctly installed, try launch the bringup:

<pre>
roslaunch hero_bringup hero_bringup.launch
</pre>

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:

[[File:first_roslaunch.png]]

You can exit the terminal.

===Connect with HeRo===
The [https://verlab.github.io/hero_common/ros/ 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.

<pre>
source devel/setup.bash
roslaunch hero_bringup hero_bringup.launch
rostopic list
</pre>

If the topics appear as expected, you can proceed to launch teleoperation mode and begin controlling HeRo directly. Make sure to adjust the <code>id</code> to match your HeRo’s ID.
<pre>
source devel/setup.bash
roslaunch hero_bringup hero_teleop.launch id:=0
</pre>

=== 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 [http://wiki.ros.org/ROS/Tutorials/CreatingPackage Create ROS Package].

You may want to create a new package or node for HeRo. To create a node quickly, navigate to <code>/ws_hero/src/hero_common/hero_examples/scripts/</code>, 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
<pre>
cd src
catkin_create_pkg hero_tutorial std_msgs rospy roscpp
cd ..
catkin_build
</pre>

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

<pre>
cd src/hero_tutorial
mkdir scripts
cd scripts
</pre>

Now, create a python file to start writing our node, we will call it tutorial.py, you need to do <code>chmod +x tutorial.py</code>. 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 <code>vellocity_controller/cmd_vel</code> topic to send velocity commands to the robot. First, lets import rospy, create our main function and initialize our new node <code>hero_walk</code>.

<pre>
#!/usr/bin/env python
import rospy

if __name__ == '__main__':
rospy.init_node('hero_walk')
</pre>

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 <code>/hero_i/vellocity_controller/cmd_vel</code> 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:

<code>rostopic info /hero_i/position_controller/cmd_vel</code>

And to see how this type of message is structured:

<code>rosmsg show geometry_msgs/Twist</code>

Now we can build our code, importing the correct message type and we will publish one time every second, or 1 Hz.
<pre>
#!/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()

</pre>

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

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

==== 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 <code>/odom</code>.

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

<pre>
#!/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()

</pre>

==== 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.

<pre>
#!/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()
</pre>

The function <code>euler_from_quaternion</code> converts quaternions into Euler angles, while <code>control</code> 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.

HeRo

2024-11-21T22:31:22Z

Caioconti: /* Controlling 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.

[[File:Hero.png|300px]]

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:
*[https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]
*[https://verlab.github.io/hero_common/ros/ 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 <code>catkin build</code> or <code>catkin_make</code> commands, choose one.

<pre>
mkdir -p ~ ws_hero/src
cd ws_hero
catkin build
</pre>

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

<pre>
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
</pre>

If you encounter issues in later steps, you may need to select the repository branch of your ROS distribution, see [https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]. You also may need to install [https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html the NVIDIA Container Toolkit]. To test if the package is correctly installed, try launch the bringup:

<pre>
roslaunch hero_bringup hero_bringup.launch
</pre>

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:

[[File:first_roslaunch.png]]

You can exit the terminal.

===Connect with HeRo===
The [https://verlab.github.io/hero_common/ros/ 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.

<pre>
source devel/setup.bash
roslaunch hero_bringup hero_bringup.launch
rostopic list
</pre>

If the topics appear as expected, you can proceed to launch teleoperation mode and begin controlling HeRo directly. Make sure to adjust the <code>id</code> to match your HeRo’s ID.
<pre>
source devel/setup.bash
roslaunch hero_bringup hero_teleop.launch id:=0
</pre>

=== 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 [http://wiki.ros.org/ROS/Tutorials/CreatingPackage Create ROS Package].

You may want to create a new package or node for HeRo. To create a node quickly, navigate to <code>/ws_hero/src/hero_common/hero_examples/scripts/</code>, 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
<pre>
cd src
catkin_create_pkg hero_tutorial std_msgs rospy roscpp
cd ..
catkin_build
</pre>

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

<pre>
cd src/hero_tutorial
mkdir scripts
cd scripts
</pre>

Now, create a python file to start writing our node, we will call it tutorial.py, you need to do <code>chmod +x tutorial.py</code>. 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 <code>vellocity_controller/cmd_vel</code> topic to send velocity commands to the robot. First, lets import rospy, create our main function and initialize our new node <code>hero_walk</code>.

<pre>
#!/usr/bin/env python
import rospy

if __name__ == '__main__':
rospy.init_node('hero_walk')
</pre>

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 <code>/hero_i/vellocity_controller/cmd_vel</code> 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:

<code>rostopic info /hero_i/position_controller/cmd_vel</code>

And to see how this type of message is structured:

<code>rosmsg show geometry_msgs/Twist</code>

Now we can build our code, importing the correct message type and we will publish one time every second, or 1 Hz.
<pre>
#!/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()

</pre>

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

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

==== 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 <code>/odom</code>.

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

<pre>
#!/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()

</pre>

==== 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.

<pre>
#!/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()
</pre>

The function <code>euler_from_quaternion</code> converts quaternions into Euler angles, while <code>control</code> is a position proportional controller that calculates linear and angular velocities.

HeRo

2024-11-21T22:28:04Z

Caioconti: /* Controlling 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.

[[File:Hero.png|300px]]

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:
*[https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]
*[https://verlab.github.io/hero_common/ros/ 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 <code>catkin build</code> or <code>catkin_make</code> commands, choose one.

<pre>
mkdir -p ~ ws_hero/src
cd ws_hero
catkin build
</pre>

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

<pre>
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
</pre>

If you encounter issues in later steps, you may need to select the repository branch of your ROS distribution, see [https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]. You also may need to install [https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html the NVIDIA Container Toolkit]. To test if the package is correctly installed, try launch the bringup:

<pre>
roslaunch hero_bringup hero_bringup.launch
</pre>

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:

[[File:first_roslaunch.png]]

You can exit the terminal.

===Connect with HeRo===
The [https://verlab.github.io/hero_common/ros/ 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.

<pre>
source devel/setup.bash
roslaunch hero_bringup hero_bringup.launch
rostopic list
</pre>

If the topics appear as expected, you can proceed to launch teleoperation mode and begin controlling HeRo directly. Make sure to adjust the <code>id</code> to match your HeRo’s ID.
<pre>
source devel/setup.bash
roslaunch hero_bringup hero_teleop.launch id:=0
</pre>

=== 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 [http://wiki.ros.org/ROS/Tutorials/CreatingPackage Create ROS Package].

You may want to create a new package or node for HeRo. To create a node quickly, navigate to <code>/ws_hero/src/hero_common/hero_examples/scripts/</code>, 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
<pre>
cd src
catkin_create_pkg hero_tutorial std_msgs rospy roscpp
cd ..
catkin_build
</pre>

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

<pre>
cd src/hero_tutorial
mkdir scripts
cd scripts
</pre>

Now, create a python file to start writing our node, we will call it tutorial.py, you need to do <code>chmod +x tutorial.py</code>. 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 <code>vellocity_controller/cmd_vel</code> topic to send velocity commands to the robot. First, lets import rospy, create our main function and initialize our new node <code>hero_walk</code>.

<pre>
#!/usr/bin/env python
import rospy

if __name__ == '__main__':
rospy.init_node('hero_walk')
</pre>

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 <code>/hero_i/vellocity_controller/cmd_vel</code> 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:

<code>rostopic info /hero_i/position_controller/cmd_vel</code>

And to see how this type of message is structured:

<code>rosmsg show geometry_msgs/Twist</code>

Now we can build our code, importing the correct message type and we will publish one time every second, or 1 Hz.
<pre>
#!/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()

</pre>

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

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

==== 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 <code>/odom</code>.

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

<pre>
#!/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()

</pre>

==== 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.

<pre>
#!/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()
</pre>

HeRo

2024-11-21T22:25:48Z

Caioconti: /* Controlling 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.

[[File:Hero.png|300px]]

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:
*[https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]
*[https://verlab.github.io/hero_common/ros/ 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 <code>catkin build</code> or <code>catkin_make</code> commands, choose one.

<pre>
mkdir -p ~ ws_hero/src
cd ws_hero
catkin build
</pre>

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

<pre>
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
</pre>

If you encounter issues in later steps, you may need to select the repository branch of your ROS distribution, see [https://verlab.github.io/hero_common/installation/ HeRo Installation Guide]. You also may need to install [https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html the NVIDIA Container Toolkit]. To test if the package is correctly installed, try launch the bringup:

<pre>
roslaunch hero_bringup hero_bringup.launch
</pre>

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:

[[File:first_roslaunch.png]]

You can exit the terminal.

===Connect with HeRo===
The [https://verlab.github.io/hero_common/ros/ 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.

<pre>
source devel/setup.bash
roslaunch hero_bringup hero_bringup.launch
rostopic list
</pre>

If the topics appear as expected, you can proceed to launch teleoperation mode and begin controlling HeRo directly. Make sure to adjust the <code>id</code> to match your HeRo’s ID.
<pre>
source devel/setup.bash
roslaunch hero_bringup hero_teleop.launch id:=0
</pre>

=== 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 [http://wiki.ros.org/ROS/Tutorials/CreatingPackage Create ROS Package].

You may want to create a new package or node for HeRo. To create a node quickly, navigate to <code>/ws_hero/src/hero_common/hero_examples/scripts/</code>, 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
<pre>
cd src
catkin_create_pkg hero_tutorial std_msgs rospy roscpp
cd ..
catkin_build
</pre>

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

<pre>
cd src/hero_tutorial
mkdir scripts
cd scripts
</pre>

Now, create a python file to start writing our node, we will call it tutorial.py, you need to do <code>chmod +x tutorial.py</code>. 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 <code>vellocity_controller/cmd_vel</code> topic to send velocity commands to the robot. First, lets import rospy, create our main function and initialize our new node <code>hero_walk</code>.

<pre>
#!/usr/bin/env python
import rospy

if __name__ == '__main__':
rospy.init_node('hero_walk')
</pre>

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 <code>/hero_i/vellocity_controller/cmd_vel</code> 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:

<code>rostopic info /hero_i/position_controller/cmd_vel</code>

And to see how this type of message is structured:

<code>rosmsg show geometry_msgs/Twist</code>

Now we can build our code, importing the correct message type and we will publish one time every second, or 1 Hz.
<pre>
#!/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()

</pre>

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

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

==== 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 <code>/odom</code>.

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

<pre>
#!/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()

</pre>

==== 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.

<pre>
#!/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

dx = x_end - x
dy = y_end - y

kr = 4 / 20
ka = 8 / 20

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):
rospy.init_node('hero_control')

# Creating publisher and subscriber
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

#rospy.loginfo("Velocidade em x: [%s]", vx)
#rospy.loginfo("Velocidade em angulo: [%s]", 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()
</pre>

HeRo

2024-11-19T20:18:03Z

Caioconti: /* Subscribing and Publishing on HeRo */

HeRo

2024-11-19T20:15:21Z

Caioconti: /* Subscribing and Publishing on HeRo */

HeRo

2024-11-19T18:19:46Z

Caioconti: /* Create ROS Package for HeRo */

HeRo

2024-11-19T18:10:20Z

Caioconti: /* Create ROS Package for HeRo */

HeRo

2024-11-19T18:04:55Z

Caioconti: /* Create ROS Package for HeRo */

HeRo

2024-11-13T18:29:19Z

Caioconti: /* Create ROS Package for HeRo */

HeRo

2024-11-13T18:18:02Z

Caioconti: /* Create ROS Package for HeRo */

HeRo

2024-11-13T18:03:48Z

Caioconti: /* New ROS Node for HeRo */

HeRo

2024-11-13T16:55:34Z

Caioconti: /* HeRo Teleoperation */

HeRo

2024-11-13T16:54:19Z

Caioconti: /* HeRo Teleoperation */

HeRo

2024-11-13T16:34:37Z

Caioconti: /* HeRo Teleoperation */

HeRo

2024-11-13T16:29:46Z

Caioconti: /* Connect with HeRo */

HeRo

2024-11-13T16:27:38Z

Caioconti: /* Set up ROS Workspace */

HeRo

2024-11-13T16:27:17Z

Caioconti: /* Connect with HeRo */

HeRo

2024-11-13T16:25:35Z

Caioconti: /* Connect with HeRo */

HeRo

2024-11-13T16:20:12Z

Caioconti: /* Connect with HeRo */

HeRo

2024-11-13T15:51:15Z

Caioconti: /* Set up ROS Workspace */

HeRo

2024-11-13T15:50:59Z

Caioconti: /* Set up ROS Workspace */

HeRo

2024-11-13T15:48:25Z

Caioconti: /* Set up ROS Workspace */

HeRo

2024-11-13T15:47:51Z

Caioconti: /* Set up ROS Workspace */

File:First roslaunch.png

2024-11-13T15:42:28Z

Caioconti: Result of the first roslaunch for the HeRo Tutorial.

Result of the first roslaunch for the HeRo Tutorial.

HeRo

2024-11-13T15:40:33Z

Caioconti: /* HeRo ROS Tutorial */

HeRo

2024-11-13T15:26:53Z

Caioconti: /* HeRo ROS Tutorial */

HeRo

2024-11-13T14:37:05Z

Caioconti: /* HeRo */

HeRo

2023-09-05T16:05:42Z

Caioconti:

HeRo

2023-09-05T16:02:27Z

Caioconti: