Fixing sd_vehicle_interface

[dylan-gonzalez]

Fix sd_vehicle_interface problems. Last semester we were having issues with the Twizy stalling on inclines, so might have to try some of the following:

• [x] talk to StreetDrone about any issues through zendesk

• [x] create a Ros package for testing that allows the user to enter a desired Twist value to control the Streetdrone

• [x] create a Foxglove configuration to show graphs of the error between command twist and current twist (/sd_current_twist)

• [ ] tune/calibrate the PID loops - create a new branch for this

• [ ] go through the control code and fix things like:

  • [ ] steering_map
  • [ ] torque_map
  • [ ] control loops

x2-3 people

[dylan-gonzalez]

@dtonda8 @jin-ramen let's schedule a kick-off meeting for this.

Are you guys free tomorrow night 5pm?

[jin-ramen]

yea i'm free 5 tmr

[dtonda8]

I'm free

[dylan-gonzalez]

Asked them about the SD_Vehicle_Interface here

[dtonda8]

I can't seem to view the post: it says "The page you were looking for doesn't exist"

Edit: Nvm. I wasn't using our MCAV account. The link works.

[dylan-gonzalez]

I edited the doc for this issue to describe the testing package we were talking about today - lmk if it needs clarification

[dylan-gonzalez]

Issues with HIL demo:

• fixed braking kicking us out
• sometimes when vehicle is stopped in CARLA, Twizy periodically spins its wheels a little bit

[jin-ramen]

SSH'ing to the Docker in the Alienware Laptop

1. SSH into the laptop

   ssh civil@192.168.11.127

2. Connect to docker

   docker exec -it mcav_autonomy_cpu bash

   Start if can't connect:

   sudo chmod 666 /var/run/docker.sock
   docker start mcav_autonomy_cpu

3. Setup the CAN, in src/external/SD-Vehicle-Interface folder:

   ./can_setup.sh

4. Launch the topics , in mcav_ws:

   source install/setup.bash
   ros2 launch sd_vehicle_interface sd_vehicle_interface.launch.xml

   Or run this for foxglove:

   source install/setup.bash
   ros2 launch twist_controller twist_controller.launch.py twist_terminal:=false

5. Turn the car into autonomous mode, then open a new terminal, connect to docker and launch Twist Controller for testing

   source install/setup.bash
   ros2 run twist_controller twist_controller

[AbBaSaMo]

@dylan-gonzalez what did you mean by "launch file to launch sd_vehicle_interface as well"?? There's already a launch file.

[dylan-gonzalez]

Yeah so use that one

[AbBaSaMo]

Ah ok. Sounded like you wanted us to write a "launch file to launch sd_vehicle_interface as well"

[AbBaSaMo]

@dylan-gonzalez

create a Ros package for testing that allows the user to enter a desired Twist value to control the Streetdrone

Is publishing twist commands via ros2 topic pub sufficient or did you specifically want a node that worked in a specific way? E.g. a node that takes in a file with a set of twist values and publishes them over time to be graphed?

[dylan-gonzalez]

Just need a good and simple way to test the Streetdrone by being able to command certain velocities and comparing with how well the Streetdrone response is But it's not too important how this testing stuff is set up so it should just be something that is quickly set up.

Using Foxglove is a good way of displaying graphs - see here for some sort of guide on how to set up. RVIZ also has some graph plugins but Foxglove might be a bit easier.

[dylan-gonzalez]

So I didn't get much time to set up some launch files with foxglove and everything. I was trying to upgrade the Alienware to Ubuntu 22 (not necessary, but I'll do it sometime this week). But here is an outline of what you have to do:

1. Download and install Foxglove Studio
2. sudo apt install ros-$ROS_DISTRO-foxglove-bridge
3. ros2 launch foxglove_bridge foxglove_bridge_launch.xml - this will run a websocket server that listens to ROS2 topics and publishes them over websockets (default port 8765)
4. run sd_vehicle_interface
5. publish twist commands
6. Open Foxglove Studio, open a websocket connection to ws://localhost:8765
7. Press the 'Add Panel' button in the top left of Foxglove Studio, add a plot, and you can add different series to the plots to visualise your data.

[dylan-gonzalez]

I think someone fixed that but I don't think they've pushed their changes uhh

But inside sd_control if you half the torque then it should work, you could probably ask @jaimasters if he's there

[jin-ramen]

https://github.com/Monash-Connected-Autonomous-Vehicle/autoware/assets/126840142/437d5127-fdec-4903-be54-fe7d61c71181

[dtonda8]

StreetDrone responded:

[dylan-gonzalez]

Hey @jin-ramen @dtonda8 where is your test_twist branch (or testing package)? I can't see it anymore?

edit: nvm, found the repository

[dylan-gonzalez]

11-12-23

Assigning @m076em to assist with PID tuning We should try contacting Hoam Chung to help us with this too We should talk to Pascal about how we can test the Twizy without a supervisor present Double check that the sd_speed_source is using the wheel encoders (rather than IMU)

[jin-ramen]

if we do:

ros2 launch twist_controller twist_controller.launch.py twist_terminal:=false sd_speed_source:=vehicle_can_speed

the wheel doesn't speed up and maintains in a constant speed.

After publishing twist value of 1, we can see from foxglove that the wheels speeds up and slows down over and over again.

[dtonda8]

UNDO_STREETDRONE_SCALING_FACTOR = 720 Previously, our twist commands were divided by 720 and the current twist was multiplied by 720

After removing UNDO_STREETDRONE_SCALING_FACTOR I (roughly) analysed a few different speeds:

twist_cmd (m/s ?)	current_twist (m/s ?)	speedometer (mph)	speedometer/current_twist	current_twist $-$ twist_cmd
0 < x < 0.05	0.09-0.12	13-15	-	-
0.1	0.2	23	115	0.1
0.2	0.3	35	116.6	0.1
0.3	0.4	46	115	0.1

FYI 1 m/s = 3.6 kmh = 2.2 mph, so scaling is still an issue. And current_twist shouldn't be larger than twist_cmd

Speeds where a lot more consistent after 0.1 m/s twist_cmd, for example, with twist_cmd.linear.x = 0.3 is shown here:

Can't seem to get speedometer below 13 with a non-zero positive twist command, even with twist_cmd.linear.x = 1e-20. Maybe it needs some rolling resistance?

Speedometer reads 0 (i.e. wheels stop spinning) only if the twist cmd is set to precisely 0.

So most likely there is some sort of minimum torque.

[dylan-gonzalez]

After talking to @owenbrooks

In UNDO_STREETDRONE_SCALING_FACTOR, 0.5 is the factor in the DBC file

100 is a Streetdrone mistake??

3.6 is kmh to mps - not sure why we're diving by 3.6 instead of multiplying

[dylan-gonzalez]

See this page for an explanation of the scaling

https://www.notion.so/monashcav/StreetDrone-Vehicle-Interface-e8c8c8a634574df4ac0d533dcbcc05f3?pvs=4

Also the Twizy might not be designed to work on non flat surfaces so we should probs just focus on testing on flat surfaces for now

[dtonda8]

I created a new scale factor NEW_SCALE_FACTOR = 51.4 to scale current_twist appropriately and scale down twist_cmd requests to whatever scale the Twizy is using (which happens to be ~51.4 times smaller that the expected twist, see last column).

twist_cmd (m/s ?)	current_twist (m/s ?)	speedometer (mph)	expected current_twist (m/s) (converted from speedometer)	Scale Factor (expected/actual)
0 < x < 0.05	0.09-0.12	13-15	-	-
0.1	0.2	23	10.28	51.41
0.2	0.3	35	15.65	52.17
0.3	0.4	46	20.56	51.4

Previously, the ANTI_FUSSINESS_TWIZY = 0.1 which happens to be the gap between the twist_cmd and current_twist. I also scaled this down to 0.00175 here. I'm guessing the ANTI_FUSSINESS_TWIZY is used to prevent exceeding the speed limit.

Whenever the current_twist was within ANTI_FUSSINESS_TWIZY from the twist_cmd, the Integration error/contribution was reset to 0. See here

I have changed this to now reset decrease the Integration error/contribution by 2.5% here.

I have also decreased the FF_Contribution_Pc by 20% here. The FF_Contribution_Pc was providing the base torque for each (probably to counteract rolling resistance), so will probably will need to revert my change here whenever testing the Twizy on the road.

I didn't use any of Owen's scaling, but it might be worth noting that his scaling factor is precisely 14 times larger than mine. New scaling seems to work, only off by $\pm 1$ mph when stabilised:

twist_cmd ($m/s^{-1}$)	speedometer (mph)
1 (2.24 mph)	3
2 (4.47 mph)	4
3 (6.71 mph)	6
4 (8.95 mph)	9

Only issue now is that the current_twist oscillates quite a bit ($\pm 3$ mph) before stabilising. We'll also need to scale up the speed index to correctly use the steer_map and torque_map.

[dylan-gonzalez]

Great work david!

Although now the current_twist and twist_cmd values match up, I don't think adding a custom scaling factor is a good thing to do in this case. Based on my previous comments, everything except the '100' term in the UNDO_STREETDRONE_SCALING_FACTOR makes sense and apparently the current_twist and twist_cmd values were matching up last semester.

It would be good though to keep a copy of these values for now somewhere so that at least we have the Twizy working as expected on blocks.

[dtonda8]

To be fair, the current_twist and twist_cmd were already kinda matching up; they were only 0.1 away from each other. My scale factor didn't actually improve this difference. It only improved when I modified the Integration error and decreased ANTI_FUSSINESS. And when the Twizy is on blocks, I could now request speeds below 13 mph.

Owen's scaling factor makes logical sense like you said. Mine is purely based on the Twizy's speedometer and just aimed to make our twist_cmd and current_twist seem more realistic. Without any scaling, twist_cmd=0.3 resulted in 46 mph. But if I remember correctly 0.3 m/s < 1 mph 😆.

I'll have another look at those notion pages and Owen's work.

[dtonda8]

everything except the '100' term in the UNDO_STREETDRONE_SCALING_FACTOR makes sense

I think Owen added this $\times 100.0$ since the StreetDrone's program divides the current velocity by 100 (for no apparent reason) here. So the $\times 100.0$ pretty much reverts this.

I now understand that linear velocity should be calculated like this (like Owen suggests here): CurrentLinearVelocity = ((double)CurrentVelocity8bit)*0.5*KPH_TO_MPS

But SD calculates the velocity like this: CurrentLinearVelocity = (CurrentVelocity8bit*KPH_TO_MPS)/100.0

If the goal of UNDO_STREETDRONE_SCALING_FACTOR is to scale SD's formula to the correct one, I'm thinking UNDO_STREETDRONE_SCALING_FACTOR should be 1000.5 = 50 (not to far off what I observed earlier), so that: `Correct_CurrentLinearVelocity = SD_CurrentLinearVelocity UNDO_STREETDRONE_SCALING_FACTOR`

But UNDO_STREETDRONE_SCALING_FACTOR is actually 3.6*100.0/0.5 = 720. SD's formula already coverts it to m/s using KPH_TO_MPS, so I'm a little confused as to why 3.6 is being used.

[owenbrooks]

Yeah looks like I've doubled up with 3.6. I'd say you're onto it with UNDO_STREETDRONE_SCALING_FACTOR = 100*0.5

[dtonda8]

Thank you for confirming. Your docs helped me a ton in understanding CAN msgs.

[dtonda8]

Are there any issues if we directly fix the formula in sd_lib_mcav.h rather than multiplying by the scaling factor? Otherwise we'll need to scale torque/steer maps in sd_control.cpp.

[owenbrooks]

Do you mean in the case where you are creating new mappings?

If you change the formula in sd_lib_mcav you’ll have to change the mappings.

The scaling factor was introduced so that we don’t have to touch the torque/steer mapping but we can still have sensible control message in our software (m/s).

So for sending control commands we take m/s, convert it to streetdrone’s weird speed, use SD’s torque map and PID gains comparing to the current_twist that they have computed. And when reporting current speed, we take SD’s weird speed, convert it to m/s just before publishing.

So essentially inside that node the speeds are all weird and outside that node they are normal.

[dtonda8]

Does the maps use m/s or SD's units? If it's configured for SD's units then you're right. To me, the maps and comments seem to suggest that they're intended to use m/s.

[owenbrooks]

Uses SD units I’m fairly sure. Yeah the comments and variable names suggest m/s but if you actually feed it m/s it tries to accelerate/decelerate too fast and locks you out of autonomous mode.

Without any scaling, the program “works” in that it gets up to a desired speed and doesn’t oscillate too much. It’s just that the input twist command has to be on the order of 0.1 m/s.

[dtonda8]

@dylan-gonzalez could you elaborate on Twizy's steering? Should we try to use Ackermann instead of angular velocity?

[dylan-gonzalez]

Since Autoware outputs an AckermannControlCommand msg type, it would be a good idea if we add support for SD vehicle interface to use that as well

[dtonda8]

I created a new branch "ackermann" for AckermannControlCommand support here. The Twizy's CAN interface uses %Angle (i.e +/-100% = steering wheel to lock left/right). 2 issues tho:

• Autoware Control outputs tire angles in radians, not as a percentage. So the ackermann branch turns tire_angle inputs as a fraction of the max steering angle, then multiplies it by a 100: (angle_request_rad / MAX_STEER_ANG) * 100. However, I couldn't find the official maximum steering angle of the Twizy. I could only estimate $40 \degree$ based on this thesis (see graph on p.19).
• AckermannControlCommand has acceleration and jerk control, which I'm not sure how to implement.