Testing Turtle Rover ROS nodes

In this tutorial, we will show how you can run and test ROS nodes for Turtle Rover.

It will be helpful to have some basic knowledge about ROS before attempting it. To learn some basics, you can head to official ROS wiki tutorials

Prepare your environment

In this tutorial, we assume you have Turtle Rover ROS packages already installed.

pageInstall Turtle Rover ROS packages

It will also be required to open multiple terminal sessions through ssh

pageConnect to the console (SSH)

Instead of creating a new ssh connection for each terminal session, we recommend using tmux to manage many sessions at once

1. Run Master Node

Before you can start other nodes, you need to have Master node running first. Let's start with sourcing our workspace

source ~/turtle_ws/devel/setup.bash

When you source the environment setup file, it creates an overlay only for the current terminal session so you need to do this in each newly opened session.

Alternatively, add this line to ~/.bashrc file (e.g. with nano). It will then be executed automatically at the start of every session.

To start Master node just type

roscore

2. Test tr_ros packages

To learn more about each node's parameters and subscribed, published topics, head to README pages in tr_ros GitHub Repository

If you found some nodes not working correctly or have some enhancement proposition, feel free to open a new issue on GitHub

2.1 tr_hat_bridge

This package contains a node that lets you communicate with Turtle Hat (RaspberryPi shield) through ROS API.

To run it, make sure you have roscore running first, and then on another terminal session, type:

rosrun tr_hat_bridge tr_hat_bridge

For Hat Bridge to work correctly, you need to have the latest firmware installed on Turtle Hat.

Now let's test some functionalities.

setting motor power

To set motor power values, you need to publish messages on tr_hat_bridge/motors topic. A message consists of 4 floating point numbers in the range [-1.0, 1.0] describing power on each wheel (the order of the wheels is RR, RL, FR, FL).

Let's test it with rostopic tool by setting half power on RR wheel

rostopic pub /tr_hat_bridge/motors std_msgs/Float32MultiArray "{data: [0.5, 0, 0, 0]}"

The wheel should start moving and won't stop until we send it another message. To stop the motors, just type:

rostopic pub /tr_hat_bridge/motors std_msgs/Float32MultiArray "{data: [0, 0, 0, 0]}"

setting servo angles

To set servo angle, you need to publish a message on topic /servoX/angle, where X is the servo channel (1, 2 or 3). For example, to set servo 1 on 90 degrees position, type:

rostopic pub /servo1/angle std_msgs/Int16 90

getting battery voltage

Battery voltage readings are published on /battery topic. To print the messages, type:

rostopic echo /battery

Parameters

There is a couple of calibration parameters for each servo:

  • servoX_min_angle

  • servoX_max_angle

  • servoX_min_duty

  • servoX_max_duty

When publishing to angle topic the value is first clamped to [min_angle, max_angle] range. Then, the angle is mapped to the duty value and sent to the Turtle Hat. The PWM in servo channels works with frequency of 50hz. A duty value of 2400 corresponds to 5% duty cycle (1ms pulse).

device parameter specifies path to the serial device used for communication with Turtle Hat. By default, it is set to /dev/ttyAMA0 (UART port on RPi 3 GPIO Header).

battery_pub_rate parameter lets you change the frequency of battery voltage queries. By default, it set to 1Hz.

2.2 tr_control

This node lets you drive the Rover using Twist messages. To test it, first make sure you have tr_hat_bridge running, then on another terminal session, run:

rosrun tr_control tr_control

Now we can publish velocity command to cmd_vel topic (Twist) or cmd_vel_stamped topic (TwistStamped). Let's write a simple Python script to test it:

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

# Initialize ROS node
rospy.init_node("test_drive")

# Create ROS publisher
cmd_pub = rospy.Publisher("cmd_vel", Twist, queue_size=1)

# Write a function that drives the Rover with specified
# linear and angular speed for 2 seconds
def drive(linear, angular):
    # Initialize ROS message object
    twist = Twist()
    twist.linear.x = linear
    twist.angular.z = angular
    
    for _ in range(20): # repeat 20 times
        cmd_pub.publish(twist) # publish message
        rospy.sleep(0.1) # sleep for 100ms

# Now let's actually test driving the Rover
# linear speed is in m/s and angular speed in rad/s 
drive(0.2, 0.0)
drive(0.0, 0.0)
drive(-0.2, 0.0)
drive(0.0, 0.0)
drive(0.0, 1.0)
drive(0.0, 0.0)
drive(0.0, -1.0)
drive(0.0, 0.0)

Copy this script to clipboard and paste it to test_drive.py using cat

cat > test_drive.py

Shift+Ins when using Putty or Ctrl+Shift+V when using Linux terminal. Hit Ctrl+D to end the file.

Add execute permission to this file:

chmod +x test_drive.py

Now, run the script by typing:

./test_drive.py

The Rover should drive forward and backward. then turn in place in left and right direction

Parameters

There is a couple of ROS parameters that can be adjusted.

wheel_radius, wheel_track and max_wheel_speed are used to evaluate motor power values based on linear and angular speeds. They are specific to the Rover model and there is no point in changing them unless you put some modifications to the wheels.

If you change differential_drive parameter to false, the driver won't support driving along a curve. It will only move straight or turn in-place depending on the greater value among linear an angular speeds.

By default, controller will stop the motors if it doesn't receive a velocity command for half a second. You can adjust this duration by changing input_timeout parameter or turning it off by setting it to 0.

If you publish velocity commands using TwistStamped messages, you can set check_timestamp parameter. When timestamp checking is enabled, controller will skip velocity command that have timestamps that differ too much from Rover's system time. For example settings it to 1.0 will skip velocity commands whose timestamps are more than 1 second in the past.

2.3 tr_teleop

This package contains nodes that let you teleoperate the Rover. They work by publishing velocity commands to cmd_vel_stamped topic, so be sure to have tr_control and tr_hat_bridge nodes running.

tr_teleop_key

This node lets you control the Rover via keyboard input. To run it just type:

rosrun tr_teleop tr_teleop_key

A help message with key mappings should appear

Keep in mind that this node is a console application and can't access keyboard events. Instead, it reads raw keyboard input in the console. It assumes the key is up when it doesn't receive an input for a specified time.

You can control this behavior by settings ~key_timeout parameter. The lower the value, the more responsive the Rover will be, but remember to set keyboard repeat delay on your system below this value. You can also disable this behavior by setting it to 0

tr_teleop_joy

This node takes joy messages (e.g. from joy_node) and converts them to velocity commands. To run it together with joy_node you can use our launch file. Simply type:

roslaunch tr_teleop joy.launch

You can configure joy axis mapping by editing config/joy.mapping.yaml file.

To test joy axis numbers you can run joy_node and display messages published in joy topic (the axes are numbered starting from 0):

rosrun joy joy_node
rostopic echo /joy

Or you can use jstest tool

jstest /dev/input/js0

To adjust speed scales, change scale_linear and scale_angular parameters in joy.launch file.

2.4 tr_bringup

This package contains a launch file which runs the following nodes:

  • tr_hat_bridge

  • tr_control

  • tr_system - provides reboot and shutdown operations through ROS topics.

  • gscam - uses gstreamer to process camera stream to jpeg images and publishes it to ROS topic.

  • web_video_server - provides a video stream of a ROS image topic that can be accessed via HTTP

  • rosbridge_server - creates WebSocket transport layer for ROS calls

To run it, just type:

roslaunch tr_bringup tr_bringup.launch

This launch file is fully compatible with leo_ui. To install the WebUI, you need to:

  • Disable tcs service if it is running:

    sudo systemctl stop turtlerover-tcs
    sudo systemctl disable turtlerover-tcs
  • Install nginx (a HTTP server):

    sudo apt update
    sudo apt install nginx
    sudo systemctl enable nginx
  • Clone leo_ui:

    cd ~
    git clone 
  • Configure nginx:

    sudo nano /etc/nginx/sites-enabled/default
    root /var/www/html; -> root /home/pi/leo_ui

Now reboot the Raspberry Pi, Run the launch file, Open Web browser and type in the link in the address bar:

http://10.0.0.1/

Last updated