-
-
Notifications
You must be signed in to change notification settings - Fork 17
Teleop Starter Project
This project will involve the creation of a drive control test bench, in which you will create an override for the joystick that we use to control the rover, send these joystick values to a backend where they will be mathematically converted to drivetrain values, then display these drivetrain values back on the GUI.
In this tutorial any code that is surrounded by *asteriks* is code that you are meant to add to the project in a certain spot. Please make sure you understand what you are adding when you add it and ask questions as you go. This project is solely for your learning so take as much time as you need to understand what is actually happening as you go through it.
First, checkout a the branch that has the starter project code:
git checkout tjn/teleop-starter
Then checkout a new branch to work on:
git checkout -b <your initials>/starter-project
example: git checkout -b jnn/starter-project
This will create a new branch so that you will not be changing your master branch in your git repository. You will not be pushing this branch, it is just there to keep it separate from your master branch. To start the project, you will launch it with a launch file much like the main base station GUI:
roslaunch mrover teleop_starter.launch
Then navigate to 127.0.0.1:8000 in your web browser
Now that you have cloned the starter code and have set up your workstation, you are ready to begin the actual project. There are a few things in the starter code to take a look at.
First let's take a look at launch/teleop_starter.launch:
<launch>
<node name="gui" pkg="mrover" type="gui_starter.sh" cwd="node"/>
</launch>
This runs gui_starter.sh, a bash script which starts the Django app (our GUI). This is different than last year where we had a rosbridge server running. Since we are not using roslibjs this year, we don't need the bridge.
Next let's take a look at package.json:
"devDependencies": {
"@webpack-cli/serve": "^1.7.0",
"babel-core": "^6.26.3",
"babel-loader": "^7.1.5",
"babel-preset-es2015": "^6.24.1",
"copy-webpack-plugin": "^4.5.2",
...
This file defines the packages that yarn installs when running the "yarnpkg install" command. Webpack is what is being used in the background to build our code and make it accessible in the browser.
Next let's look at the frontend/src/components directory, as well as router/index.js. MenuPage.vue defines the main menu that can be found at the default route of our program.
DriveControls.vue has some basic code to get values from the Joystick that is normally used to operate the rover.
interval = window.setInterval(() => {
const gamepads = navigator.getGamepads()
for (let i = 0; i < 4; i++) {
const gamepad = gamepads[i]
if (gamepad) {
if (gamepad.id.includes('Logitech')) {
// -1 multiplier to make turning left a positive value
// Both left_right axis and twisting the joystick will turn
this.rotational = -1 * (gamepad.axes[JOYSTICK_CONFIG['left_right']] + gamepad.axes[JOYSTICK_CONFIG['twist']])
// Range Constraints
if (this.rotational > 1) {
this.rotational = 1
}
else if (this.rotational < -1) {
this.rotational = -1
}
// forward on joystick is -1, so invert
this.linear = -1 * gamepad.axes[JOYSTICK_CONFIG['forward_back']]
}
}
}
}, updateRate*1000)
It's encouraged that you explore other files but these are the essential spots that are important to the starter code. Let's begin working on the motor sim!
First we are going to add a separate route, /motor_sim for the drive controls vue module. A route is simply a separate page under the same website. For example, for project management we use https://github.com, but our repo is located at https://github.com/umrover/mrover-ros, "umrover/mrover-ros" is the route to our repo.
To add our route we are going to add a MenuButton to the MenuPage component.
<div class="box row">
<MenuButton link="#/motor_sim" name="Motor Simulator"></MenuButton>
</div>
This will create a menu button that will link to the motor_sim route. The template section of the code should now look like:
<template>
<div class="wrapper">
<div class="box header">
<img src="/static/mrover.png" alt="MRover" title="MRover" width="48" height="48" />
<h1>Mrover Teleop Training</h1>
<div class="spacer"></div>
</div>
<div class="box row">
<MenuButton link="#/motor_sim" name="Motor Simulator"></MenuButton>
</div>
</div>
</template>
Now we need to add the MenuButton to the components of the vue module. Add the following line to the top of the script section of the code
*import MenuButton from './MenuButton.vue';*
export default {
...
then add a components section to the export section
export default {
name: 'MenuPage',
*components: {
MenuButton
}*
}
Your menu will now look like this:
You'll likely notice that when you click the button it takes you to a blank page. This is because we need to define the route. Open the src/router/index.js file. We are going to add a route that looks very much like the one that is already present for the menu.
import { createWebHistory, createRouter } from "vue-router";
import Menu from "../components/MenuPage.vue";
*import DriveControls from "../components/DriveControls.vue";*
const routes = [
{
path: "/",
name: "Menu",
component: Menu,
},
*{
path: '/motor_sim',
name: 'Motor Sim',
component: DriveControls
},*
];
const router = createRouter({
history: createWebHistory(),
routes,
});
export default router;
"Drive Controls" should now be displayed on the /motor_sim route.
Next we are going to send the values from the joysticks that are currently being read in in DriveControls.vue to a ros joystick topic. To do this, first we are going to create a message file. Create a new directory in the top-level of your teleop_training workspace called "msg" and inside this folder create a new file called Joystick.msg.
Your workspace should now look like this:
Inside the Joystick.msg file we will define the members of the message we are going to be sending:
float64 forward_back
float64 left_right
Note that these message files use types according to the ROS message format defined here.
Next, we will need to add our message to CMakeLists.txt in the teleop starter folder so that catkin can build our new messages. Add the Joystick message to the add_message_files function in the starter project CMakeLists file.
add_message_files(
DIRECTORY
${CMAKE_CURRENT_LIST_DIR}/msg
FILES
Joystick.msg
)
Now navigate back to the catkin_ws directory and run "catkin build" again to add your message to the mrover package.
Now we need to send our joystick values that we are getting to a ROS Topic so that other ROS nodes can access it. This will be different than in previous years, since we're implementing Django. Now, to send the joystick values we need to send them through a Websocket connection. Django will be on the other side of this connection and receive the joystick values. From there, it will take those values and publish it to a ROS topic so other nodes can access it. To start, let's start in the DriveControls script section and add a websocket.
The object will be declared in the data section and then assigned in the created() function so that only one instance is created.
export default {
data () {
return {
rotational: 0,
linear: 0,
*socket: null* <-- Declaring variable
}
},
...
const JOYSTICK_CONFIG = {
'left_right': 0,
'forward_back': 1,
'twist': 2,
'dampen': 3,
'pan': 4,
'tilt': 5
}
*this.socket = new WebSocket('ws://127.0.0.1:8000/ws/drive-controls');*
const updateRate = 0.05;
...
There are a few things to note about this code block. For one, all values declared in the data() section need to be prefixed with "this." when accessing them at any time. This is because they are member variables of the Vue component and thus can be accessed in any context and are not just local to a certain function or context (It's a very common javascript syntax).
Now you need to send joystick values from Vue to Django:
beforeUnmount: function () {
window.clearInterval(interval);
},
methods: {
*sendToROS(msg) {
this.socket.send(JSON.stringify(msg));
}*
},
created: function () {
...
const joystickData = {
'type': 'joystick',
'forward_back': this.linear,
'left_right': this.rotational,
}
*this.sendToROS(joystickData);*
}
}
}
}, updateRate*1000)
...
Now that the joystick values are sent to Django, we need to publish these values to a topic in ROS. Head to backend/consumers.py. Here you can see a fairly empty Consumer class.
import json
from channels.generic.websocket import JsonWebsocketConsumer
class GUIConsumer(JsonWebsocketConsumer):
def connect(self):
self.accept()
def disconnect(self, close_code):
pass
def receive(self, text_data):
"""
Receive message from WebSocket.
"""
pass
This class connects to Websockets, receives JSON string objects (our joystick values), and can disconnect from Websockets. To better organize our receiving of data, all messages we send will have a type.
Add the following to the receive() method:
def receive(self, text_data):
"""
Receive message from WebSocket.
"""
*message = json.loads(text_data)
if message['type'] == "joystick":
self.handle_joystick_message(message)*
This type corresponds to a function to be called. You can see that before we sent the data to this backend, we specified a "joystick" type. Therefore, we need to create this function:
*import rospy
from mrover.msg import Joystick*
class GUIConsumer(JsonWebsocketConsumer):
...
*def handle_joystick_message(self, msg):
fb = msg["forward_back"]
lr = msg["left_right"]
pub = rospy.Publisher('/joystick', Joystick, queue_size=100)
message = Joystick()
message.forward_back = float(fb)
message.left_right = float(lr)
pub.publish(message)*
As you can see, we get the values from the parameter and then create a publisher that publishes data to the /joystick topic. The data being sent will be type Joystick (the custom message we created earlier). We assign the message variables data and then finally publish the message.
Currently, our interface can send Vue data and publish to ROS, but what about the other way? Next, we'll create a subscriber to listen to the /joystick topic and then send that data to Vue through the websocket.
class GUIConsumer(JsonWebsocketConsumer):
def connect(self):
*self.sub = rospy.Subscriber('/joystick', Joystick, self.joystick_callback)*
self.accept()
def disconnect(self, close_code):
*self.sub.unregister()*
*def joystick_callback(self, msg):
forward_back = msg.forward_back
left_right = msg.left_right*
This establishes a subscriber once in the connect() method of the consumer which in turn creates a callback. In this callback, we are going to translate the forward_back/left_right variables into motor outputs (left/right) that can be sent to each respective drivetrain of the rover (ESW team takes over from there). In order for ESW to know what the motor outputs are, we need to publish the left/right values to a topic. Create a new message called WheelCmd.msg to send values to the wheels.
float64 left
float64 right
Refer back to making the Joystick message if you get stuck making this wheel message.
Now Import your messages at the top of the python script
from mrover.msg import Joystick, WheelCmd
This is the part of the project where, if you would like, you can write custom code. There are many different ways that one could handle the translation of the values from joystick to motor outputs. Here is some simple code that will accomplish this based on this stackexchange post, but keep in mind that this is not the only way to do this and the purpose of the project you are building is to test different algorithms for this.
def joystick_callback(self,msg):
forward_back = msg.forward_back
left_right = msg.left_right
#Scaling multiplier to adjust values if needed
K = 1
left = K * (forward_back + left_right)
right = K * (forward_back - left_right)
#Ensure values are [-1,1] for each motor
if abs(left) > 1:
left = left/abs(left)
if abs(right) > 1:
right = right/abs(right)
# Send output to WebSocket
self.send(text_data=json.dumps({
'type': 'wheel_cmd',
'left': left,
'right': right
}))
#publish motor output to ros topic
pub = rospy.Publisher("/wheel_cmd", WheelCmd,queue_size=100)
message = WheelCmd()
message.left = left
message.right = right
pub.publish(message)
Whether you choose to use this code or not, we will need to send these left and right values to Vue and the /wheel_cmd topic.
The final step will be to display the output values that are given for the motors in order to observe the outputs of whatever algorithm you chose.
Head back to DriveControls.vue to begin on this.
The first step to doing this will be adding variables to hold the most recent wheel_cmd values.
export default {
data () {
return {
rotational: 0,
linear: 0,
*left: 0,
right: 0,*
socket: null
}
},
Now we will display these values on the GUI.
<div>
<p>Drive Controls</p>
<div>
<p>Left Motor Output: {{left}}</p>
<p>Right Motor Output: {{right}}</p>
</div>
</div>
Putting things in double curly braces will output the member value of the Vue component with the same name. Note that since this only works for member components, you do not need to use "this."
Since we sent the wheel_cmd values from Django to Vue, we need to assign the left and right values as each message is received. This can be done through the onmessage() function of the websocket.
created: function () {
const JOYSTICK_CONFIG = {
'left_right': 0,
'forward_back': 1,
'twist': 2,
'dampen': 3,
'pan': 4,
'tilt': 5
}
this.socket = new WebSocket('ws://127.0.0.1:8000/ws/drive-controls');
*this.socket.onmessage = (msg) => {
msg = JSON.parse(msg.data)
if(msg.type == "wheel_cmd"){
this.left = msg.left;
this.right = msg.right;
}
}*
...
You may have been asking yourself through this starter project "I don't have a joystick, how am I going to test this code?" Fear not, ROS has an answer. The rostopic command line utility allows us to send our own custom messages to any ros topic using only the terminal.
You can send a Joystick messages by entering:
rostopic pub /joystick mrover/Joystick '{forward_back : 1.0, left_right: 0.0}'
rostopic is a super helpful tool for debugging, the other most important command to know about is rostopic echo. After sending your joystick message run the following command in another terminal:
rostopic echo /wheel_cmd
This should display the same values that are displayed on the GUI end of things. You can learn more about rostopic here.
Your final Motor sim screen should look something like this:
It is highly reccomended that you use rostopic and send several /joystick messages and think about whether the outputs make sense for our rover. Remember that the rover turns by having different values sent to each side of the drivetrain and not by steering like a car.
If possible, you can go try this out with the same Logitech joystick we use to control the drivetrain on the rover!
With this, you have finished the teleop starter project and hopefully should feel comfortable writing code that can be used to control the rover. Congratulations!