# Navigation with ArUco Tags

**URL:** <https://forum.hello-robot.com/t/navigation-with-aruco-tags/483>\
**Category:** Knowledge Base\
**Created:** [September 17, 2022, 12:31am UTC](https://forum.hello-robot.com/t/navigation-with-aruco-tags/483 "2022-09-17T00:31:58Z")\
**Posts on this page:** 4\
**Page:** 1

<div class="post-metadata">

**Author:** ![hello-garv](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/hello-garv/32/296_2.png) [@hello-garv](https://forum.hello-robot.com/u/hello-garv)\
**Post date:** [September 17, 2022, 12:31am UTC](https://forum.hello-robot.com/t/navigation-with-aruco-tags/483/1 "2022-09-17T00:31:58Z")

</div>

Hello Stretch Community!

I am a masters student with Northwestern’s MS in Robotics program, and this summer, I have been interning with the fantastic Hello Robot crew. I was brought on to help develop for Stretch specifically with use cases for high-needs end-users in mind. One feature in particular I thought might be of interest to the broader Stretch community is automating the base to return to a specific location and orientation using ArUco markers.

This feature will continue to be developed and can be found in the [features/aruco\_navigation](https://github.com/hello-robot/stretch_ros/tree/feature/aruco_navigation) branch. In stretch core, I added a new launch file called [aruco\_navigation.launch](https://github.com/hello-robot/stretch_ros/blob/feature/aruco_navigation/stretch_core/launch/aruco_navigation.launch). This launch file will open an RVIZ window to visualize ArUco frames and 2D Nav Goals. I also launch the move\_base node, which takes advantage of our recent improvements to the NavStack with the Global Planner and DWA local planner. Make sure you have the most recent updates to the stretch\_navigation [configuration files](https://github.com/hello-robot/stretch_ros/tree/feature/aruco_navigation/stretch_navigation/config).

The [aruco\_navigation.py](https://github.com/hello-robot/stretch_ros/blob/feature/aruco_navigation/hello_helpers/src/hello_helpers/aruco_navigation.py) script in stretch\_ros/hello\_helpers provides functions to save a base position relative to an ArUco tag and then return to that exact position.

Once the aruco\_navigation.launch file is launched,

```auto
roslaunch stretch_core aruco_navigation.launch

```

simple python scripts can be constructed with the hello\_helpers functions for a wide variety of applications.

One great use case is auto-docking. In this example, I used the functions from hello\_helpers/aruco\_navigation.py to automatically navigate the base into the docking station.

First, the tag for the docking station must be added to the list of known aruco tags in the [aruco marker dictionary](https://github.com/hello-robot/stretch_ros/blob/master/stretch_core/config/stretch_marker_dict.yaml). I chose “docking,” but you can assign any string in the “name” field.

```auto
'aruco_marker_info':
    '245':
        'length_mm': 88.0
        'use_rgb_only': False
        'name': 'docking'
        'link': None

```

If you are unsure of the ID number for the tag you are working with, you can echo the /aruco/marker\_array topic

```auto
rostopic echo /aruco/marker_array

```

in a new terminal window after launching aruco\_navigation.launch and hold the tag so it is visible to the realsense camera to find the the number in the “id” field.

Once we have a recognizable tag, we can create our first entry into the pose dictionary. For this example, I manually positioned my robot half a meter directly in front of the docking station. Then, I used save\_pose() to remember this position relative to the docking station. The first argument is a name to identify the pose, and the second is the string that identifies that aruco tag.

```auto
    import hello_helpers.aruco_navigation as an
    
    aruco = an.ArucoNavigationNode()
    aruco.save_pose("ready to dock","docking")

```

The save pose function will pan the head until the RealSense camera sees the requested tag. Once the head stops moving, a new dictionary entry will be saved to a JSON file in the stretch\_core/config directory. The JSON file allows for poses to be used across sessions.

Now, we are ready to navigate to the saved pose. go\_to\_pose() will look up the previously saved pose in the pose dictionary and pan the head to find the same tag, then transform the pose into the current map frame and send a navigation goal.

```auto
    import hello_helpers.aruco_navigation as an
    
    aruco = an.ArucoNavigationNode()
    success = aruco.go_to_pose("ready to dock")

```

![docking](https://canada1.discourse-cdn.com/flex030/uploads/hello_robot2/original/1X/6bb07490dc9ff2cd49c6eb18ebca1bcc6fbbd7fc.gif)  
_3x Speed_

Once the navigation is complete, the base should be lined up and ready for docking. To finish docking, send a twist straight backwards.

```auto
import rospy
import time
from geometry_msgs.msg import Twist

cmd_vel_pub = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=1)

if success: 
    twist = Twist()
    twist.linear.x = -0.05 #5 cm/s backwards
    t = 0.5/0.05 #.5 meters / 0.05 m/s = 10s 
    start_time = rospy.Time.now()
    while rospy.Time.now() < start_time + rospy.Duration.from_sec(t):
        cmd_vel_pub.publish(twist)
        time.sleep(.25)

```

![docking2](https://canada1.discourse-cdn.com/flex030/uploads/hello_robot2/original/1X/a5bae3ce6d663021230b9fd91af7710fef0d4ed7.gif)  
_3x Speed_

I have found these functions to be a great starting point for automating tasks, and I hope I can inspire some cool new features from the community!

Happy tinkering!

Anna Garverick

---

<div class="post-metadata">

**Author:** ![roboor](https://avatars.discourse-cdn.com/v4/letter/r/3bc359/32.png) [@roboor](https://forum.hello-robot.com/u/roboor)\
**Post date:** [May 25, 2023, 6:45am UTC](https://forum.hello-robot.com/t/navigation-with-aruco-tags/483/2 "2023-05-25T06:45:37Z")

</div>

Hi Anna,

Thank you for your great input to realise the docking! I am looking forward to set it up and try it on my own 🙂

Is there any command that returns the battery status? That would be practical check if the robot is actually charging, so that the docked correctly.

With `stretch_robot_battery_check.py` the charging status is not included.

Best,  
Noor

---

<div class="post-metadata">

**Author:** ![Mohamed\_Fazil](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/mohamed_fazil/32/261_2.png) [@Mohamed\_Fazil](https://forum.hello-robot.com/u/Mohamed_Fazil)\
**Post date:** [May 26, 2023, 12:29am UTC](https://forum.hello-robot.com/t/navigation-with-aruco-tags/483/3 "2023-05-26T00:29:13Z")

</div>

Hi @roboor ,

You are right, we don’t provide the charging/discharging status for the robot and therefore did not include it to the `stretch_robot_battery_check.py` tool.  
But Stretch 2 has a mechanical switch in its charge port that is tied to `robot_status['pimu']['charger_connected']` which allows you to detect if the charger gets connected.  
Stretch RE1 doesn’t have this switch and therefore this value will always be zero. You can refer to this [forum post](https://forum.hello-robot.com/t/getting-charge-state-of-the-robot/597/2).

Best,  
Mohamed Fazil

---

<div class="post-metadata">

**Author:** ![roboor](https://avatars.discourse-cdn.com/v4/letter/r/3bc359/32.png) [@roboor](https://forum.hello-robot.com/u/roboor)\
**Post date:** [May 26, 2023, 5:24am UTC](https://forum.hello-robot.com/t/navigation-with-aruco-tags/483/4 "2023-05-26T05:24:42Z")

</div>

Hi @Mohamed_Fazil,

That’s great info! Thank you very much.

Best,  
Noor