Skip to content
GitLab
Commit 9067f4b8 authored by Simon M. Haller-Seeber's avatar Simon M. Haller-Seeber
Browse files

Added working sphero rvr ros stack

parent 65bc56e1
Hide whitespace changes
Inline Side-by-side
README.md
View file @ 9067f4b8
......@@ -7,11 +7,10 @@ This is very alpha.
- The Base Jetson Nano image is based on https://github.com/pythops/jetson-nano-image/
- ROS Noetic is installed from the Ubuntu Repoistories
## The RVR Base:
- ROS RVR is based on: https://github.com/markusk/rvr
- The SDK might be exchanged with: https://bitbucket.org/rmerriam/rvr-cpp
- https://github.com/strean/rvr_ros and maybe this ros control stack
- or ROS2 : https://github.com/lomori/spherorvr-ros2
## The Sphero RVR Base:
- This repository is build upon: https://github.com/strean/rvr_ros and the free Sphero RVR CPP library from https://bitbucket.org/rmerriam/rvr-cpp.
- ROS Noetic does not provide ros-noetic-serial - this is build from https://github.com/wjwwood/serial. (Note: this is not the rosserial library)
- At a later stage we might have a look at the ROS2 implementation from lomori: https://github.com/lomori/spherorvr-ros2
## The Jetson Board (build instructions):
......
catkin_ws/src/CMakeLists.txt 0 → 120000
View file @ 9067f4b8
/opt/ros/noetic/share/catkin/cmake/toplevel.cmake
\ No newline at end of file
catkin_ws/src/rvr/launch/ground_control_center.launch deleted 100644 → 0
View file @ 65bc56e1
<?xml version="1.0" encoding="UTF-8"?>
<!--
Launch file for controlling and observing the robots status remote.
-->
<launch>
<!-- Battery surveilance. -->
<!-- <node name="rqt_plot" pkg="rqt_plot" type="rqt_plot" output="screen"/> -->
<!-- Joystick control -->
<include file="$(find rvr)/launch/joystick_control.launch" pass_all_args="true"/>
<!-- Camera live view (image viewer) -->
<!-- <node name="image_view" pkg="image_view" type="image_view" args="image:=/camera" output="screen"/> -->
<!-- Laser scanner livew view -->
<!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find rvr)/rviz/laserMK.rviz" output="screen"/> -->
</launch>
catkin_ws/src/rvr/launch/joystick_control.launch deleted 100644 → 0
View file @ 65bc56e1
<?xml version="1.0" encoding="UTF-8"?>
<!--
Listening to a joystick and controlling the robot directly (remote).
This part has to be started on a different computer than the robot.
The joystick/gamepas has to be connected to that computer, not the robot.
-->
<launch>
<!--
This nodes listens to joystick/gamepad input.
Use the D-Pad buttons on the joystick/gamepad to control the motors.
-->
<node name="joy_node" pkg="joy" type="joy_node" output="screen"/>
<!--
This nodes takes the joystick input and sends out motor control messages.
The motor control on a hardware level is started by 'roslaunch rvr motor_server' on the robot.
-->
<node name="joy_motor_listener" pkg="rvr" type="joy_motor_listener.py" output="screen"/>
</launch>
catkin_ws/src/rvr/launch/keyboard_control_test.launch deleted 100644 → 0
View file @ 65bc56e1
<?xml version="1.0" encoding="UTF-8"?>
<!-- Listens to a teleop_twist_keyboard node and prints out the data/messages. -->
<launch>
<!-- This nodes listens to keyboard input -->
<node name="teleop_twist_keyboard" pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" output="screen"/>
<!-- This nodes takes the keyboard input and performs any action -->
<node name="keyboard_listener" pkg="rvr" type="keyboard_listener.py" output="screen"/>
</launch>
catkin_ws/src/rvr/launch/motor_server.launch deleted 100644 → 0
View file @ 65bc56e1
<?xml version="1.0" encoding="UTF-8"?>
<!--
Listening to a joystick and controlling the robot directly (remote).
This launch file is for the robot.
-->
<launch>
<!--
Setting the hostname of the robot. This is to optional skip hardware setup steps later.
-->
<param name="rvr/hostname" type="String" value="rvrmate" />
<!-- Controlling the motor on a hardware level -->
<node name="motor_server" pkg="rvr" type="motor_server.py" output="screen"/>
</launch>
catkin_ws/src/rvr/launch/motor_server_minibot.launch deleted 100644 → 0
View file @ 65bc56e1
<?xml version="1.0" encoding="UTF-8"?>
<!--
Listening to a joystick and controlling the robot directly (remote).
This launch file is for the robot.
-->
<launch>
<!--
Setting the hostname of the robot. This is to optional skip hardware setup steps later.
-->
<param name="rvr/hostname" type="String" value="rvrmate" />
<!-- Controlling the motor on a hardware level -->
<node name="motor_server_minibot" pkg="rvr" type="motor_server_minibot.py" output="screen"/>
</launch>
catkin_ws/src/rvr/launch/rvr.launch deleted 100644 → 0
View file @ 65bc56e1
<?xml version="1.0" encoding="UTF-8"?>
<!--
Controls the whole robot and has to run on the "RVR" (Raspberry Pi).
-->
<launch>
<!--
Setting the hostname of the robot. This is to optional skip hardware setup steps later.
-->
<param name="rvr/hostname" type="String" value="rvrmate" />
<!--
Setting the diffent speeds for the robot.
-->
<param name="rvr/drivingSpeed" type="int" value="75" />
<param name="rvr/turnSpeed" type="int" value="90" />
<!--
For the Adafruit motor hat the motor speed is 0-255.
s.a. https://learn.adafruit.com/adafruit-motor-shield-v2-for-arduino/library-reference
-->
<param name="rvr/minMotorSpeed" type="int" value="0" />
<param name="rvr/maxMotorSpeed" type="int" value="255" />
<!-- Publishing "/tf" tf/tfMessages for the transformation tree. -->
<!-- <node name="tf_broadcaster" pkg="rvr" type="tf_broadcaster.py" output="screen"/> -->
<!-- Publishing the battery voltage as Float32. -->
<!-- <node name="battery_publisher" pkg="rvr" type="battery_publisher.py" output="screen"/> -->
<!-- Publishing "odom" nav_msgs/Odometry for the local planner (navigation stack). -->
<!-- <node name="odom_publisher" pkg="rvr" type="odom_publisher.py" output="screen"/> -->
<!-- Controlling the motor on a hardware level -->
<node name="motor_server" pkg="rvr" type="motor_server.py" output="screen"/>
<!-- Base controller listens to "cmd_vel" geometry_msgs/Twist messages and publishes motor control messages (for the motor_server node). -->
<node name="base_controller" pkg="rvr" type="base_controller.py" output="screen"/>
<!-- Camera support -->
<!-- <node pkg="rvr" type="camera" name="rvr_camera" output="screen"/> -->
<!-- This is for the laser Hokuyo URG04lx laser rangefinder. -->
<!-- <node name="laser" pkg="urg_node" type="urg_node" args="/dev/ttyLaser" output="screen"/> -->
<!-- Joystick/gamepad support -->
<arg name="joy_config" default="xbox" />
<!-- Device path for the joystick -->
<arg name="joy_dev" default="/dev/input/js0" />
<arg name="config_filepath" default="$(find teleop_twist_joy)/config/$(arg joy_config).config.yaml" />
<node pkg="joy" type="joy_node" name="joy_node">
<param name="dev" value="$(arg joy_dev)" />
<param name="deadzone" value="0.3" />
<param name="autorepeat_rate" value="4" />
<param name="coalesce_interval" value="0.5" />
</node>
<!--
This node takes the joystick input, and converts it to Twist message cmd_vel.
The cmd_vel will be used by node base_controller which communicates with the motor node.
-->
<node pkg="teleop_twist_joy" name="teleop_twist_joy" type="teleop_node">
<rosparam command="load" file="$(arg config_filepath)" />
<param name="scale_linear" value="0.8" />
<param name="scale_angular" value="0.8" />
</node>
</launch>
catkin_ws/src/rvr/launch/test.launch deleted 100644 → 0
View file @ 65bc56e1
<?xml version="1.0" encoding="UTF-8"?>
<!--
Controls the whole robot and has to run on the "RVR" (Raspberry Pi).
-->
<launch>
<!--
Setting the hostname of the robot. This is to optional skip hardware setup steps later.
-->
<param name="rvr/hostname" type="String" value="rvrmate" />
<!--
Setting the diffent speeds for the robot.
-->
<param name="rvr/drivingSpeed" type="int" value="75" />
<param name="rvr/turnSpeed" type="int" value="90" />
<!-- Testign the Shero RVR -->
<node name="test" pkg="rvr" type="test.py" output="screen"/>
</launch>
catkin_ws/src/rvr/lib/README.md deleted 100644 → 0
View file @ 65bc56e1
(The Sphero SDK/lib will be symlinked here.)
See instructions here: https://github.com/markusk/rvr
catkin_ws/src/rvr/nodes/base_controller.py deleted 100755 → 0
View file @ 65bc56e1
#!/usr/bin/env python3
# coding=utf-8
"""
This is the ROS node for the RVR (https://direcs.de).
It expects "cmd_vel" geometry_msgs/Twist messages to control the robots motors.
It will then publish messages like "FORWARD, BACKWARD, LEFT, RIGHT, STOP" with the speed,
which will be received by a "motor_server" node. The latter is responsible for
controlling the motors with lowlevel I2C commands on a Raspberry Pi.
This node can (also) be controlled via keyboard with the
teleop_twist_keyboard or teleop_twist_joy node.
Usage:
roslaunch rvr keyboard_motor_control.launch
or
roslaunch rvr teleop_joy.launch
Author: Markus Knapp, 2020
Website: https://direcs.de
"""
import rospy
from geometry_msgs.msg import Twist
# The rvr package has the motor server, which listens to messages from here
from rvr.srv import *
# Getting robot parameters
rospy.loginfo('Getting parameters for robot.')
# min speed of the motors (i.e. 0-255 for adafruit motor shield).
minMotorSpeed = rospy.get_param('/rvr/minMotorSpeed')
rospy.loginfo('Using minMotorSpeed %s.', minMotorSpeed)
# max speed of the motors (i.e. 0-255 for adafruit motor shield).
maxMotorSpeed = rospy.get_param('/rvr/maxMotorSpeed')
rospy.loginfo('Using maxMotorSpeed %s.', maxMotorSpeed)
# node init
rospy.init_node('keyboard_listener', anonymous=False)
# Service 'motor' from motor_server.py ready?
rospy.loginfo("Waiting for service 'motor'")
rospy.wait_for_service('motor')
# this will execute the "drive" command
def drive(direction, speed):
# Service 'motor' from motor_server.py ready?
rospy.wait_for_service('motor')
# Send driving direction to motor
try:
# Create the handle 'motor_switcher' with the service type 'Motor'.
# The latter automatically generates the MotorRequest and MotorResponse objects.
motor_switcher = rospy.ServiceProxy('motor', Motor)
# the handle can be called like a normal function
rospy.loginfo("Switching motors to %s @ speed %s.", direction, speed)
response = motor_switcher(direction, speed)
# show result
rospy.loginfo(rospy.get_caller_id() + ' says result is %s.', response.result)
except rospy.ServiceException as e:
rospy.logerr("Service call for 'motor' failed: %s", e)
def callback(data):
# print out received message from the teleop_twist_keyboard
# rospy.loginfo(rospy.get_caller_id() + ' received x=%s', data.linear.x)
# rospy.loginfo(rospy.get_caller_id() + ' received z=%s', data.angular.z)
# map joystick value to motor speed (i.e. 0.7 to 255)
# @todo: check how to read this value from the launch file
#scaleLinear = rospy.get_param('/joy_node/scaleLinear')
scaleLinear = 0.8
factor = scaleLinear/maxMotorSpeed
# which command was received/key was pressed?
if (data.linear.x > 0.0) and (data.angular.z == 0.0): # @todo: implement curve travel with the help of angular.z
speed = int(data.linear.x/factor)
rospy.loginfo("FORWARD.")
drive("FORWARD", speed)
# , key
elif (data.linear.x < 0.0) and (data.angular.z == 0.0):
speed = int(data.linear.x/factor) * -1
rospy.loginfo("BACKWARD.")
drive("BACKWARD", speed)
# j key
elif (data.linear.x == 0.0) and (data.angular.z > 0.0):
speed = int(data.angular.z/factor)
rospy.loginfo("LEFT .")
drive("LEFT", speed)
# l key
elif (data.linear.x == 0.0) and (data.angular.z < 0.0):
speed = int(data.angular.z/factor) * -1
rospy.loginfo("BACKWARD.")
drive("RIGHT", speed)
# k key
elif (data.linear.x == 0.0) and (data.angular.z == 0.0):
rospy.loginfo("STOP.")
drive("STOP", 0)
def listener():
# subscribe the message cmd_vel
rospy.Subscriber('cmd_vel', Twist, callback)
# Ready
rospy.loginfo("Ready. Control me via navigation stack/joystick/keyboard now (cmd_vel).")
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
if __name__ == '__main__':
listener()
catkin_ws/src/rvr/nodes/getBatteryState.py deleted 100755 → 0
View file @ 65bc56e1
#!/usr/bin/python3
# coding=utf-8
import os
import sys
sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '../lib/')))
import asyncio
from sphero_sdk import SpheroRvrAsync
from sphero_sdk import SerialAsyncDal
from sphero_sdk import BatteryVoltageStatesEnum as VoltageStates
# for LEDs:
from sphero_sdk import Colors
from sphero_sdk import RvrLedGroups
loop = asyncio.get_event_loop()
rvr = SpheroRvrAsync(
dal=SerialAsyncDal(
loop
)
)
async def main():
""" This program demonstrates how to retrieve the battery state of RVR.
"""
await rvr.wake()
# Give RVR time to wake up
await asyncio.sleep(2)
# turn all LEDs OFF
#await rvr.set_all_leds(
# led_group=RvrLedGroups.all_lights.value,
# led_brightness_values=[color for _ in range(10) for color in Colors.off.value]
#)
# battery in %
battery_percentage = await rvr.get_battery_percentage()
print("Battery percentage: ", battery_percentage['percentage'], "%")
# battery state
battery_voltage_state = await rvr.get_battery_voltage_state()
print('Voltage state: ', battery_voltage_state['state'])
# battery state
state_info = '[{}, {}, {}, {}]'.format(
'{}: {}'.format(VoltageStates.unknown.name, VoltageStates.unknown.value),
'{}: {}'.format(VoltageStates.ok.name, VoltageStates.ok.value),
'{}: {}'.format(VoltageStates.low.name, VoltageStates.low.value),
'{}: {}'.format(VoltageStates.critical.name, VoltageStates.critical.value)
)
print('Voltage states: ', state_info)
# Delay to show LEDs change
# await asyncio.sleep(1)
# set LEDs depending on the battery state
if battery_percentage['percentage'] > 66:
# All LEDs to green [okay]
await rvr.set_all_leds(
led_group=RvrLedGroups.all_lights.value,
led_brightness_values=[color for x in range(10) for color in [0, 255, 0]]
)
elif battery_percentage['percentage'] > 33:
# All LEDs to orange [low]
await rvr.set_all_leds(
led_group=RvrLedGroups.all_lights.value,
led_brightness_values=[color for x in range(10) for color in [255, 165, 0]]
)
elif battery_percentage['percentage'] < 33:
# All LEDs to red [critical]
await rvr.set_all_leds(
led_group=RvrLedGroups.all_lights.value,
led_brightness_values=[color for x in range(10) for color in [255, 0, 0]]
)
await rvr.close()
if __name__ == '__main__':
try:
loop.run_until_complete(
main()
)
except KeyboardInterrupt:
print('\nProgram terminated with keyboard interrupt.')
# turn all LEDs OFF
#await rvr.set_all_leds(
# led_group=RvrLedGroups.all_lights.value,
# led_brightness_values=[color for _ in range(10) for color in Colors.off.value]
#)
loop.run_until_complete(
rvr.close()
)
finally:
if loop.is_running():
loop.close()
catkin_ws/src/rvr/nodes/joy_motor_listener.py deleted 100755 → 0
View file @ 65bc56e1
#!/usr/bin/env python3
# coding=utf-8
"""
This is my listener for the joy_node. It listens on the topic 'joy' and prints out some information.
It then switches on motors on my Raspberry when the D-Pad is used on the gamepad.
This needs the motor_server to be run on the Raspberry Pi. Like this:
Usage
-----
Raspberry Pi:
1. roslaunch rvr motor_server. (This starts also the roscore on this computer automatically).
Another Ubuntu machine:
1. export ROS_MASTER_URI=http://hostname-of-your-pi:11311/ from the robot. I.E.:
export ROS_MASTER_URI=http://rvr:11311/
2. Set joystick device (if different) to js0. I.E.:
rosparam set joy_node/dev "/dev/input/js2"
3. roslaunch rvr joystick_control.launch
Author: Markus Knapp, 2020
Website: https://direcs.de
"""
import rospy
from std_msgs.msg import String
from sensor_msgs.msg import Joy
# name of the package(!).srv
from rvr.srv import *
# Getting robot parameters
rospy.loginfo('Getting parameters for robot.')
# speed of the motors (0-255).
drivingSpeed = rospy.get_param('/rvr/drivingSpeed')
rospy.loginfo('Using drivingSpeed %s.', drivingSpeed)
# the speed when turning the bot can be higher if needed (higher friction)
turnSpeed = rospy.get_param('/rvr/turnSpeed')
rospy.loginfo('Using turnSpeed %s.', turnSpeed)
# Service 'motor' from motor_server.py ready?
rospy.loginfo("Waiting for service 'motor'")
rospy.wait_for_service('motor')
# this will execute the "drive" command
def drive(direction, speed):
# Send driving direction to motor
try:
# Create the handle 'motor_switcher' with the service type 'Motor'.
# The latter automatically generates the MotorRequest and MotorResponse objects.
motor_switcher = rospy.ServiceProxy('motor', Motor)
# the handle can be called like a normal function
rospy.loginfo("Switching motors to %s @ %s.", direction, speed)
response = motor_switcher(direction, speed)
# show result
rospy.loginfo(rospy.get_caller_id() + ' says result is %s.', response.result)
except rospy.ServiceException, e:
rospy.logerr("Service call for 'motor' failed: %s", e)
def callback(joy):
"""
# debug messages
rospy.loginfo("+++ Found Axes: %s joystick axes+++", len(joy.axes))
rospy.loginfo("+++ Found %s joystick buttons +++", len(joy.buttons))
rospy.loginfo("axis 0: %s", joy.axes[0])
rospy.loginfo("axis 1: %s", joy.axes[1])
rospy.loginfo("axis 2: %s", joy.axes[2])
rospy.loginfo("axis 3: %s", joy.axes[3])
rospy.loginfo("axis 4: %s", joy.axes[4])
rospy.loginfo("axis 5: %s", joy.axes[5])
rospy.loginfo("-------------------")
"""
# which button was pressed?
# D-Pad, vertikal up or XBOX controller cross up
# check if we really using my xbox controller with >10 axes
# otherwise get an exception accessing the array out of bound
# @todo: implement a better solution!! if (len(joy.axes) > 10):
if (joy.axes[5] == 1.0) or (joy.axes[7] == 1.0):
rospy.loginfo("FORWARD button pressed.")
drive("FORWARD", drivingSpeed)
# D-Pad, vertikal down
elif (joy.axes[5] == -1.0) or (joy.axes[7] == -1.0):
rospy.loginfo("BACKWARD button pressed.")
drive("BACKWARD", drivingSpeed)
# D-Pad, horizontal left or XBOX controller cross left
elif (joy.axes[4] == 1.0) or (joy.axes[6] == 1.0):
rospy.loginfo("LEFT button pressed.")
drive("LEFT", turnSpeed)
# D-Pad, horizontal right or XBOX controller cross right
elif (joy.axes[4] == -1.0) or (joy.axes[6] == -1.0):
rospy.loginfo("RIGHT button pressed.")
drive("RIGHT", turnSpeed)
# red button on my gamepad or XBOX controller red button B
elif (joy.buttons[10] == 1.0) or (joy.buttons[1] == 1.0):
rospy.loginfo("RED button pressed.")
drive("STOP", 0)
#else:
# rospy.logwarn("Wrong joystick/gamepad: not enough axes!")
def listener():
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'joy_listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('joy_listener', anonymous=True)
# subscribe the joy(stick) topic
rospy.Subscriber('joy', Joy, callback)
# Ready
rospy.loginfo("Ready. Press a button on your joystick D-Pad now.")
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
if __name__ == '__main__':
listener()
catkin_ws/src/rvr/nodes/keyboard_listener.py deleted 100755 → 0
View file @ 65bc56e1
#!/usr/bin/env python3
# coding=utf-8
"""
Simple keyboard_listener demo that listens to geometry_msgs/Twist
published by the 'cmd_vel' topic of the teleop_twist_keyboard node
and prints out the received data/messsage.
That's it.
Usage:
roslaunch rvr keyboard_control.launch
Author: Markus Knapp, 2020
Website: https://direcs.de
"""
import rospy
from geometry_msgs.msg import Twist
def callback(data):
# print out received message from the teleop_twist_keyboard
rospy.loginfo(rospy.get_caller_id() + ' x=%s\n', data.linear.x)
rospy.loginfo(rospy.get_caller_id() + ' y=%s\n', data.linear.y)
rospy.loginfo(rospy.get_caller_id() + ' z=%s\n', data.angular.z)
def listener():
# In ROS, nodes are uniquely named. The anonymous=True flag
# means that rospy will choose a unique name for this listener node
rospy.init_node('keyboard_listener', anonymous=True)
# subscribe the message cmd_vel
rospy.Subscriber('cmd_vel', Twist, callback)
# Ready
rospy.loginfo("Ready. Control me via keyboard now.")
# spin() simply keeps python from exiting until this node is stopped