Feature/gazebo-sim-fixes #27

MaxxWilson · 2023-09-09T18:42:54Z

PR Summary

PR Link: develop...feature/gazebo-sim-fixes
Issue Link: #6

Description

Develop a Plugin which mocks the V5SensorUpdate and V5ActuatorCommand ROS topics in Gazebo

Reviewers

Tag reviewers.

Required: @MaxxWilson
Optional: @JakeWendling @xwilson03

Changelog

Adds a diff drive gazebo model so that we could split testing the autonomy, controls, and estimation stack from developing the swerve model.
Adds EKF from robot_localization pkg. Somewhat tuned. Need to handle increase of estimated covariance over time.

Reviewer Guide

Most changes are in the ghost_sim pkg.

Field walls were removed since it caused a not-obvious bug: when given a cmd_vel, the robot collided with the walls and continued to attempt to achieve a cmd_vel, causing the wheels to slip, changing the odometry readings. Ultimately, the odometry encoder and the imu messages disagreed, causing the EKF to diverge.

Note that the EKF in this PR uses 3 "sensors": gazebo IMU plugin, actual truth model data as "odometry" data, and gazebo diff drive plugin odometry configured as "WORLD" instead of "ENCODER". Therefore, the data given to the EKF is very ideal data. Performance should be monitored with more noisy data.

Testing

Automatic

N/A

Manual

launch the plotjuggler, diff drive, robot_localization stack using:
ros2 launch ghost_sim test_tank_sim.launch.py
in a separate terminal launch
ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 5.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"
or
ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 1.0}}"
for sinsusoid motion.
If subscribed to the odometry/filtered topic, the pose and twist should update with slight deviation from the gz/model_states pose and twist.

Documentation

Here is there package used for robot localization

Checklist

Confirmed all tests pass on a clean build
Added reviewers in Github
Posted PR Summary to Discord PR's Channel
Ran uncrustify on any modified C++ files
Ran Colcon Lint for any modified CMakeLists.txt or Package.xml

Here's a video of the diff drive EKF

sensor updates publishing across topic Untested since gz motor command interface not defined

Adds encoder to joints and motor to joint cmd interface

Updates gazebo sim fixes - up to date with new develop refactor

of dummy v5actuator cmd data

must double check units

but very slowly double check units

validate with plots

attempts to fix bugs

no gz diff drive plugin loaded

Able to send vel cmds Need to validate vel/pos tracking

test_tank launch file for live streaming data

need to validate filtered odom output with plotjuggler

validated with plotjuggler

…_GHOST into feature/gazebo-sim-fixes

Updates gazebo sim fixes branch with develop

…n diff drive is stationary. Must be tuned again for swerve drive.

melissajcruz · 2024-01-03T21:52:14Z

This branch has V5Actuator update interface that lacks rigorous testing framework to validate.

In an attempt to overcome this, a test differential drive model was implemented without the V5actuator update interface using a differential drive gazebo plugin with the intention of combining the urdf model with the V5actuator update interface. However, development deviated from testing the V5actuator update interface to implementing the test diff drive model for developing an autonomy, estimation, control stack.

So, this branch has a diff drive with a robot localization stack implemented via a somewhat tuned EKF. I think the main purpose of this branch has deviated from the original purpose and should be merged back into dev.

I'm not sure what the status is of the sensor and actuator interfaces. We should discuss this!

.gitmodules

MaxxWilson · 2024-01-04T23:19:30Z

03_ROS/ghost_ros/CMakeLists.txt

-install(TARGETS
-  foxglove_diagnostics_node
-  DESTINATION lib/${PROJECT_NAME})
+# add_executable(foxglove_diagnostics_node


Was planning to delete this FYI, bc Foxglove seems sorta unsupported. Will handle in separate PR.

It's been commented out in the CMake.

03_ROS/ghost_sim/include/ghost_sim/test_publisher_v5_actuator_cmd.hpp

MaxxWilson · 2024-01-04T23:23:19Z

03_ROS/ghost_sim/include/ghost_sim/v5_robot_plugin.hpp

@@ -35,9 +36,13 @@ class V5RobotPlugin : public gazebo::ModelPlugin {

 protected:
 	void jointToEncoderTransform();
-	ghost_msgs::msg::V5EncoderState wrapEncoderMsg(const int col_index);
-	ghost_msgs::msg::V5SensorUpdate populateSensorMsg();
+	Eigen::VectorXd motorToJointTransform(Eigen::VectorXd motor_data);


Header file methods need comment blocks. Autofills on my VSCode by doing /** and then enter

MaxxWilson · 2024-01-04T23:24:00Z

03_ROS/ghost_sim/include/ghost_sim/v5_robot_plugin.hpp

-	ghost_msgs::msg::V5EncoderState wrapEncoderMsg(const int col_index);
-	ghost_msgs::msg::V5SensorUpdate populateSensorMsg();
+	Eigen::VectorXd motorToJointTransform(Eigen::VectorXd motor_data);
+	Eigen::VectorXd jointToMotorTransform(Eigen::VectorXd joint_data);


Should pass these by const ref if they aren't changing, and ref if they are changing. Try to avoid copying non-trivial types.

Changed to const ref.

03_ROS/ghost_sim/src/v5_robot_plugin.cpp

MaxxWilson · 2024-01-04T23:25:58Z

03_ROS/ghost_sim/src/v5_robot_plugin.cpp

+	ghost_msgs::msg::V5Joystick joystick_msg_;
+
+	// Constants
+	float DEG_TO_RAD = M_PI / 180;


can these be 1) either const or constexpr, and 2) moved to ghost_util into a "types.hpp" header. Might want to use somewhere else.

This is already defined in 01_Libraries/ghost_common/include/ghost_common/util/angle_util.hpp so I just replaced the constants with the constants that already exist.

03_ROS/ghost_sim/src/v5_robot_plugin.cpp

MaxxWilson · 2024-01-05T00:36:26Z

03_ROS/ghost_sim/src/v5_robot_plugin.cpp

 }

+// Preserves order of joints listed in xacro
 void V5RobotPlugin::getJointStates(){


could this be updateJointStates(), since it isn't returning anything?

MaxxWilson · 2024-01-05T00:39:29Z

Looks very solid! And we have diff drive actually working in sim, which is dope.

There may be a lot of small interface changes on upcoming robot_interface overhaul PR, but those will be on my end lol.

Is there a way to add unit testing so we know we can load the plugin and it can load a default config file? I think there is a GazeboTestFixture example somewhere.

Can we also add videos to this PR of the working thing for documentation sake?

testing is in development

Make sure base model and diff drive plugin params agree!

melissajcruz · 2024-01-24T01:45:02Z

Final Update:
Added working EKF video to PR description.
After speaking with Maxx, determined Gazebo Test Fixture used seems to be doing some dirty deallocation at the end of the test scripts. So, if your output looks something like this:

xacro: in-order processing became default in ROS Melodic. You can drop the option.
Allow generated test description files to be moved to temp testing folder
Killing gazebo processes
gzserver: no process found
UpdateCTestConfiguration  from :/home/melcruz/VEXU_GHOST/build/ghost_sim/CTestConfiguration.ini
Parse Config file:/home/melcruz/VEXU_GHOST/build/ghost_sim/CTestConfiguration.ini
 Add coverage exclude regular expressions.
UpdateCTestConfiguration  from :/home/melcruz/VEXU_GHOST/build/ghost_sim/CTestConfiguration.ini
Parse Config file:/home/melcruz/VEXU_GHOST/build/ghost_sim/CTestConfiguration.ini
Test project /home/melcruz/VEXU_GHOST/build/ghost_sim
Constructing a list of tests
Done constructing a list of tests
Updating test list for fixtures
Added 0 tests to meet fixture requirements
Checking test dependency graph...
Checking test dependency graph end
test 1
    Start 1: test_diff_drive

1: Test command: /usr/bin/python3.10 "-u" "/opt/ros/humble/share/ament_cmake_test/cmake/run_test.py" "/home/melcruz/VEXU_GHOST/build/ghost_sim/test_results/ghost_sim/test_diff_drive.gtest.xml" "--package-name" "ghost_sim" "--output-file" "/home/melcruz/VEXU_GHOST/build/ghost_sim/ament_cmake_gtest/test_diff_drive.txt" "--command" "/home/melcruz/VEXU_GHOST/build/ghost_sim/test_diff_drive" "--gtest_output=xml:/home/melcruz/VEXU_GHOST/build/ghost_sim/test_results/ghost_sim/test_diff_drive.gtest.xml"
1: Test timeout computed to be: 120
1: -- run_test.py: invoking following command in '/home/melcruz/VEXU_GHOST/build/ghost_sim':
1:  - /home/melcruz/VEXU_GHOST/build/ghost_sim/test_diff_drive --gtest_output=xml:/home/melcruz/VEXU_GHOST/build/ghost_sim/test_results/ghost_sim/test_diff_drive.gtest.xml
1: [==========] Running 1 test from 1 test suite.
1: [----------] Global test environment set-up.
1: [----------] 1 test from GazeboRosDiffDriveTest
1: [ RUN      ] GazeboRosDiffDriveTest.testBringUp
1: [Msg] Waiting for master.
1: [Msg] Connected to gazebo master @ http://127.0.0.1:11345
1: [Msg] Publicized address: 172.25.171.127
1: [Msg] Loading world file [/home/melcruz/VEXU_GHOST/install/ghost_sim/share/ghost_sim/urdf/spin_up.world]
1: [Wrn] [Event.cc:61] Warning: Deleting a connection right after creation. Make sure to save the ConnectionPtr from a Connect call
1: [Dbg] [ServerFixture.cc:211] ServerFixture load in 2.9 seconds, timeout after 600 seconds
1: /home/melcruz/VEXU_GHOST/install/ghost_sim/share/ghost_sim
1: ===Loading diff drive model===
1: [INFO] [1706057065.861770653] [gazebo_ros_state]: Publishing states of gazebo models at [/model_states]
1: [INFO] [1706057065.862330012] [gazebo_ros_state]: Publishing states of gazebo links at [/link_states]
1: [INFO] [1706057066.114238301] [gazebo_ros_diff_drive]: Wheel pair 1 separation set to [0.300000m]
1: [INFO] [1706057066.114302665] [gazebo_ros_diff_drive]: Wheel pair 1 diameter set to [0.200000m]
1: [INFO] [1706057066.115204849] [gazebo_ros_diff_drive]: Subscribed to [/cmd_vel]
1: [INFO] [1706057066.116717739] [gazebo_ros_diff_drive]: Advertise odometry on [/odom]
1: [INFO] [1706057066.120285924] [gazebo_ros_diff_drive]: Publishing odom transforms between [odom] and [base_link]
1: [INFO] [1706057066.120324185] [gazebo_ros_diff_drive]: Publishing wheel transforms between [base_link], [left_wheel_hinge] and [right_wheel_hinge]
1: [Dbg] [ServerFixture.cc:130] ServerFixture::Unload
1: -- run_test.py: return code -11
1: -- run_test.py: generate result file '/home/melcruz/VEXU_GHOST/build/ghost_sim/test_results/ghost_sim/test_diff_drive.gtest.xml' with failed test
1: -- run_test.py: verify result file '/home/melcruz/VEXU_GHOST/build/ghost_sim/test_results/ghost_sim/test_diff_drive.gtest.xml'
1/1 Test #1: test_diff_drive ..................***Failed    5.30 sec

0% tests passed, 1 tests failed out of 1

Label Time Summary:
gtest    =   5.30 sec*proc (1 test)

Total Test time (real) =   5.31 sec

The following tests FAILED:
          1 - test_diff_drive (Failed)
Errors while running CTest
Output from these tests are in: /home/melcruz/VEXU_GHOST/build/ghost_sim/Testing/Temporary/LastTest.log
Use "--rerun-failed --output-on-failure" to re-run the failed cases verbosely.

The diff drive test passed! Since there's no fix for now, we will just use a ros2 launch command to launch the sim and visually inspect that everything works.

The tests have some bad deallocation so you should visually test the diff drive in GZ instead of running this test script.

…re/gazebo-sim-fixes

MaxxWilson · 2024-02-01T16:57:37Z

2/1 Test Failures:

small cleanup passing tests, approved!

MaxxWilson

Good with this once submodules are fixed

03_ROS/deprecated/ghost_ros/CMakeLists.txt

.gitmodules

…_GHOST into feature/gazebo-sim-fixes

melissajcruz

lgtm!

melissajcruz added 2 commits August 30, 2023 20:33

Compiles and runs sim with gz joint to encoder

a094d71

sensor updates publishing across topic Untested since gz motor command interface not defined

Does not compile

df54c24

Adds encoder to joints and motor to joint cmd interface

MaxxWilson assigned melissajcruz Sep 9, 2023

melissajcruz and others added 10 commits September 9, 2023 14:40

Merge branch 'develop' into feature/gazebo-sim-fixes

f57772c

Updates gazebo sim fixes - up to date with new develop refactor

Merge branch 'develop' into feature/gazebo-sim-fixes

898d8de

Sim runs. Robot control unstable b/c

2bf19d1

of dummy v5actuator cmd data

cleans std::cout

50ad98b

v5actuatorcmd test pubs v5 mtr cmd

f348572

must double check units

sim rotates wheel module

e3e05bc

but very slowly double check units

Swerve moves in sim with pseudo turret. Must

ca31c0b

validate with plots

Adds swerve kinematics and joystick msg

ce89eca

joystick publishes, sim torques dont work

f53d6a8

joystick devel pt 2

ce74e6e

MaxxWilson mentioned this pull request Oct 16, 2023

adds field mesh to gazebo with collision #31

Open

MaxxWilson and others added 14 commits October 17, 2023 09:33

adds symlink-install option to build script

e7c7cf0

modifes joystick channel to be configurable from launch file

f7266e7

attempts to fix bugs

b532fe3

uncomment swerve jacobian

a452f41

Merge pull request #34 from VEXU-GHOST/bug-fix/sim-node-debugging

8670ac8

attempts to fix bugs

test diff drive model in gz.

6effd71

no gz diff drive plugin loaded

Test diff drive spawns in gz

736f7ef

Able to send vel cmds Need to validate vel/pos tracking

Adds plotjuggler to

d6c8dfc

test_tank launch file for live streaming data

imu topic not being published to for robot_localization pkg

459135d

robot localization node launches

f47efc8

need to validate filtered odom output with plotjuggler

robot localization EKF node launches

a8fe4bf

validated with plotjuggler

Merge branch 'feature/gazebo-sim-fixes' of github.com:VEXU-GHOST/VEXU…

fee13b2

…_GHOST into feature/gazebo-sim-fixes

Merge pull request #38 from VEXU-GHOST/develop

933e6f5

Updates gazebo sim fixes branch with develop

EKF works with test diff drive if given cmd_vel. Need to test EKF whe…

30b8449

…n diff drive is stationary. Must be tuned again for swerve drive.

Cleaning not needed files

b795ecf

MaxxWilson commented Jan 5, 2024

View reviewed changes

melissajcruz added 5 commits January 5, 2024 13:41

Adds changes from PR comments

3b53311

Addresses PR comments and adds gz tests

0391de2

testing is in development

update git ignore

772d3c4

Testing script runs, fails some assertions.

9f43781

Fixed weird x angular velocty:

34a612f

Make sure base model and diff drive plugin params agree!

melissajcruz and others added 5 commits January 25, 2024 16:34

Changes diff drive tests and adds test script

e5c7b67

The tests have some bad deallocation so you should visually test the diff drive in GZ instead of running this test script.

Merge branch 'develop' of github.com:VEXU-GHOST/VEXU_GHOST into featu…

5afc0cc

…re/gazebo-sim-fixes

Refactors sim stack after hardware refactor

1ae2a9a

Merge branch 'develop' into feature/gazebo-sim-fixes

52f0c0a

Cleaning

2b1e79f

MaxxWilson added 10 commits February 1, 2024 12:08

moves world files to world folder

4ce8984

fixes last world file move ref

bf10964

change world path to worlds

4d9f1dc

moves test files to /tmp/

5108bac

small cleanup passing tests, approved!

removes boilerplate testing from ghost_tank

27ebb98

test fixing merge issues

cf04081

more reverting merge changes

fe2ad1b

merge fixes v3

4674e42

final merge fixes?

5bd6656

revert swerve changes

aecba23

MaxxWilson commented Feb 1, 2024

View reviewed changes

03_ROS/deprecated/ghost_ros/CMakeLists.txt Outdated Show resolved Hide resolved

.gitmodules Outdated Show resolved Hide resolved

melissajcruz added 3 commits February 1, 2024 17:28

Changes dep to add pj and pjros

cf22447

Merge branch 'feature/gazebo-sim-fixes' of github.com:VEXU-GHOST/VEXU…

8c3b84a

…_GHOST into feature/gazebo-sim-fixes

Cleaning

2027038

melissajcruz approved these changes Feb 1, 2024

View reviewed changes

MaxxWilson merged commit 04d091e into develop Feb 1, 2024

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Feature/gazebo-sim-fixes #27

Feature/gazebo-sim-fixes #27

MaxxWilson commented Sep 9, 2023 •

edited by melissajcruz

Loading

melissajcruz commented Jan 3, 2024

MaxxWilson Jan 4, 2024

melissajcruz Jan 5, 2024

MaxxWilson Jan 4, 2024

melissajcruz Jan 5, 2024

MaxxWilson Jan 4, 2024

melissajcruz Jan 5, 2024

MaxxWilson Jan 4, 2024

melissajcruz Jan 5, 2024

MaxxWilson Jan 5, 2024

melissajcruz Jan 5, 2024

MaxxWilson commented Jan 5, 2024 •

edited

Loading

melissajcruz commented Jan 24, 2024 •

edited

Loading

MaxxWilson commented Feb 1, 2024

MaxxWilson left a comment

melissajcruz left a comment

Feature/gazebo-sim-fixes #27

Feature/gazebo-sim-fixes #27

Conversation

MaxxWilson commented Sep 9, 2023 • edited by melissajcruz Loading

PR Summary

Description

Reviewers

Changelog

Reviewer Guide

Testing

Automatic

Manual

Documentation

Checklist

melissajcruz commented Jan 3, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

MaxxWilson commented Jan 5, 2024 • edited Loading

melissajcruz commented Jan 24, 2024 • edited Loading

MaxxWilson commented Feb 1, 2024

MaxxWilson left a comment

Choose a reason for hiding this comment

melissajcruz left a comment

Choose a reason for hiding this comment

MaxxWilson commented Sep 9, 2023 •

edited by melissajcruz

Loading

MaxxWilson commented Jan 5, 2024 •

edited

Loading

melissajcruz commented Jan 24, 2024 •

edited

Loading