Feature/gazebo-sim-fixes

03_ROS/ghost_sim/CMakeLists.txt

@@ -19,6 +19,7 @@ add_compile_options(-Wno-dev)
 ########################
 set(DEPENDENCIES
   ament_cmake
+  ament_cmake_gtest
   Eigen3
   yaml-cpp
   rclcpp
@@ -50,13 +51,15 @@ ${PYTHON_INCLUDE_DIRS}
 include_directories(${INCLUDE})
 ament_export_include_directories(${INCLUDE})
 
-link_directories(${gazebo_dev_LIBRARY_DIRS})
+include_directories(${GTEST_INCLUDE_DIRS})
 
+link_directories(${gazebo_dev_LIBRARY_DIRS})
 
 ###################
 ##### TARGETS #####
 ###################
 # Gazebo Motor Plugin
+
 # add_library(gazebo_motor_plugin SHARED src/gazebo_motor_plugin.cpp)
 # ament_target_dependencies(gazebo_motor_plugin
 #   ${DEPENDENCIES}
@@ -69,6 +72,7 @@ link_directories(${gazebo_dev_LIBRARY_DIRS})
 #     $<INSTALL_INTERFACE:include>)
 # ament_export_libraries(gazebo_motor_plugin)
 
+
 # Gazebo Joint PID Plugin
 # add_library(gazebo_joint_pid_plugin SHARED src/gazebo_joint_pid_plugin.cpp)
 # ament_target_dependencies(gazebo_joint_pid_plugin
@@ -90,6 +94,26 @@ link_directories(${gazebo_dev_LIBRARY_DIRS})
 #     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
 #     $<INSTALL_INTERFACE:include>)
 # ament_export_libraries(gazebo_joint_pid_plugin)
+enable_testing()
+ament_add_gtest(test_diff_drive test/test_diff_drive.cpp
+    # Long timeout because has to run gazebo several times
+    TIMEOUT
+    120)
+
+target_link_libraries(test_diff_drive
+  gazebo_test_fixture
+)
+
+ament_target_dependencies(test_diff_drive
+  "builtin_interfaces"
+  "gazebo_dev"
+  "gazebo_msgs"
+  "geometry_msgs"
+  "rclcpp"
+  "sensor_msgs"
+  "std_msgs"
+  "nav_msgs"
+)
 
 # Gazebo V5 Robot Plugin
 # add_library(v5_robot_plugin SHARED src/v5_robot_plugin.cpp)
@@ -112,29 +136,25 @@ link_directories(${gazebo_dev_LIBRARY_DIRS})
 #   ground_truth_pose_publisher
 #   DESTINATION lib/${PROJECT_NAME})
 
-# Node to test new V5ActuactorCommand communication
-# add_executable(test_publisher_v5_actuator_cmd
-#   test/test_publisher_v5_actuator_cmd.cpp
-# )
-# ament_target_dependencies(test_publisher_v5_actuator_cmd
-#   ${DEPENDENCIES}
+################
+### Install ####
+################
+# install(
+#   PROGRAMS launch/test_tank_sim.launch.py
+#   DESTINATION lib/${PROJECT_NAME}
 # )
-# install(TARGETS
-#   test_publisher_v5_actuator_cmd
-#   DESTINATION lib/${PROJECT_NAME})
 
-# ################
-# ### Install ####
-# ################
 install(
-  DIRECTORY include/
+  DIRECTORY include
   DESTINATION include
 )
 
 install(DIRECTORY
   launch
   urdf
+  worlds
   config
   DESTINATION share/${PROJECT_NAME})
 
+
 ament_package()

03_ROS/ghost_sim/config/v5_sim_config.yaml

@@ -1,8 +1,10 @@
 motor_names:
-  - "DRIVE_LEFT_FRONT_MOTOR"
-  - "DRIVE_LEFT_BACK_MOTOR"
-  - "DRIVE_RIGHT_FRONT_MOTOR"
-  - "DRIVE_RIGHT_BACK_MOTOR"
-  - "DRIVE_BACK_LEFT_1_MOTOR"
-  - "DRIVE_BACK_RIGHT_1_MOTOR"
+  - "DRIVE_LEFT_FRONT_LEFT_MOTOR"
+  - "DRIVE_LEFT_FRONT_RIGHT_MOTOR"
+  - "DRIVE_LEFT_BACK_LEFT_MOTOR"
+  - "DRIVE_LEFT_BACK_RIGHT_MOTOR"
+  - "DRIVE_RIGHT_FRONT_LEFT_MOTOR"
+  - "DRIVE_RIGHT_FRONT_RIGHT_MOTOR"
+  - "DRIVE_RIGHT_BACK_LEFT_MOTOR"
+  - "DRIVE_RIGHT_BACK_RIGHT_MOTOR"
   - "TURRET_MOTOR"

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

@@ -33,6 +33,10 @@ class testPublisherV5ActuatorCmd : public rclcpp::Node {
 
 	void publishData();
 	rclcpp::Publisher<ghost_msgs::msg::V5ActuatorCommand>::SharedPtr output_pub_;
+	std::array<std::string, 8> motors_{"DRIVE_LEFT_FRONT_LEFT_MOTOR",
+		                           "DRIVE_LEFT_FRONT_RIGHT_MOTOR", "DRIVE_LEFT_BACK_LEFT_MOTOR", "DRIVE_LEFT_BACK_RIGHT_MOTOR",
+		                           "DRIVE_RIGHT_FRONT_LEFT_MOTOR", "DRIVE_RIGHT_FRONT_RIGHT_MOTOR", "DRIVE_RIGHT_BACK_LEFT_MOTOR",
+		                           "DRIVE_RIGHT_BACK_RIGHT_MOTOR"};
 	ghost_msgs::msg::V5MotorCommand populateMotorCmd(const int loop_index);
 
 	// private:

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

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <algorithm>
+#include <Eigen/QR>
 #include <memory>
 #include <random>
 #include <vector>
@@ -34,12 +35,49 @@ class V5RobotPlugin : public gazebo::ModelPlugin {
 	void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override;
 
 protected:
+	/**
+	 * 	Converts joint data from Gazebo to encoder data using sensor jacobian matrix
+	 */
 	void jointToEncoderTransform();
-	ghost_msgs::msg::V5EncoderState wrapEncoderMsg(const int col_index);
-	ghost_msgs::msg::V5SensorUpdate populateSensorMsg();
-	void getJointStates();
 
-	/// Optional callback to be called at every simulation iteration.
+	/**
+	 * Transforms motor data (motor pos, or vel, or accel), to the location of the corresponding joint in swerve model
+	 */
+	Eigen::VectorXd motorToJointTransform(const Eigen::VectorXd& motor_data);
+
+	/**
+	 * Transforms joint data (joint pos, or vel, or accel), to the location of the corresponding motor in swerve model
+	 */
+	Eigen::VectorXd jointToMotorTransform(const Eigen::VectorXd& joint_data);
+
+	/**
+	 * Updates joint commands given motor actuator input command
+	 */
+	void updateMotorController();
+
+	/**
+	 * Wraps encoder matrix into V5Encoder state msg[]
+	 */
+	void wrapEncoderMsg();
+
+	/**
+	 * Populates sensor msg using current encoder msg
+	 */
+	void populateSensorMsg();
+
+	/**
+	 * Updates joint angles and velocities from gazebo
+	 */
+	void updateJointStates();
+
+	/**
+	 * Applies command joint torques to gazebo links
+	 */
+	void applySimJointTorques();
+
+	/**
+	 * Optional callback to be called by Gazebo at every simulation iteration.
+	 */
 	void OnUpdate();
 
 private:

03_ROS/ghost_sim/launch/joystick.launch.py

@@ -1,16 +1,29 @@
 from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+
 from launch_ros.actions import Node
 
-def generate_launch_description():
-    return LaunchDescription([
-        Node(
+def launch_setup(context, *args, **kwargs):
+
+    channel_id = context.perform_substitution(LaunchConfiguration('channel_id'))
+    joy_linux_node = Node(
             package = 'joy_linux',
             executable = 'joy_linux_node',
             name = 'joystick_node',
             parameters= [{
-                "dev": "/dev/input/js0"
+                "dev": "/dev/input/js" + channel_id
             }]
-        ),
+        )
+
+    return [joy_linux_node]
+
+def generate_launch_description():
+
+
+    return LaunchDescription([
+        DeclareLaunchArgument(name='channel_id', default_value='1'),
+        OpaqueFunction(function = launch_setup),
         Node(
             package = 'teleop_twist_joy',
             executable = 'teleop_node',

03_ROS/ghost_sim/launch/new_v5_msg_config_test.launch.py

@@ -63,13 +63,13 @@ def launch_setup(context, *args, **kwargs):
 def generate_launch_description():
     # Load relevant filepaths
     gazebo_ros_share_dir = get_package_share_directory('gazebo_ros')
-    ghost_ros_share_dir = get_package_share_directory('ghost_ros')
+    ghost_ros_share_dir = get_package_share_directory('ghost_ros_interfaces')
     ghost_sim_share_dir = get_package_share_directory('ghost_sim')
 
     home_dir = os.path.expanduser('~')
-    ghost_ros_base_dir = os.path.join(home_dir, "VEXU_GHOST", "ghost_ros")
+    ghost_ros_base_dir = os.path.join(home_dir, "VEXU_GHOST", "ghost_ros_interfaces")
 
-    world_file = os.path.join(ghost_sim_share_dir, "urdf", "spin_up.world")
+    world_file = os.path.join(ghost_sim_share_dir, "worlds", "default.world")
     rviz_config_path = os.path.join(ghost_ros_share_dir, 'rviz/urdf_config.rviz')
 
     # Simulator (Doesn't launch Simulator GUI by default, use CLI Arg "sim_gui" for debugging)

03_ROS/ghost_sim/launch/start_sim.launch.py

@@ -35,7 +35,22 @@ def launch_setup(context, *args, **kwargs):
         output='screen',
         parameters=[{'use_sim_time': False}, {"robot_description": doc}])
 
-    return [gazebo_ros, robot_state_publisher]
+    # Joystick (Only launched if joystick CLI arg is set to True)
+    joy_launch_description = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                ghost_sim_share_dir,
+                "launch",
+                "joystick.launch.py"
+            )
+        ),
+        condition=launch.conditions.IfCondition(LaunchConfiguration("use_joy")),
+        launch_arguments={
+            'channel_id': LaunchConfiguration("channel_id"),
+        }.items()
+    )
+
+    return [gazebo_ros, robot_state_publisher, joy_launch_description]
 
 
 def generate_launch_description():
@@ -45,9 +60,9 @@ def generate_launch_description():
     ghost_sim_share_dir = get_package_share_directory('ghost_sim')
 
     home_dir = os.path.expanduser('~')
-    ghost_ros_base_dir = os.path.join(home_dir, "VEXU_GHOST", "ghost_ros")
+    ghost_ros_base_dir = os.path.join(home_dir, "VEXU_GHOST", "03_ROS", "ghost_ros")
 
-    world_file = os.path.join(ghost_sim_share_dir, "urdf", "spin_up.world")
+    world_file = os.path.join(ghost_sim_share_dir, "world", "default.world")
     rviz_config_path = os.path.join(ghost_ros_share_dir, 'rviz/urdf_config.rviz')
 
     # Simulator (Doesn't launch Simulator GUI by default, use CLI Arg "sim_gui" for debugging)
@@ -69,6 +84,12 @@ def generate_launch_description():
         name='ground_truth_pose_publisher',
     )
 
+    v5_actuator_cmd_publisher = Node(
+        package='ghost_sim',
+        executable = 'test_publisher_v5_actuator_cmd',
+        name = 'test_publisher_v5_actuator_cmd',
+    )
+
     # Launch RVIZ Display as primary GUI interface
     rviz_node = Node(
         package='rviz2',
@@ -77,43 +98,32 @@ def generate_launch_description():
         arguments=['-d', rviz_config_path],
     )
 
-    # Joystick (Only launched if joystick CLI arg is set to True)
-    joy_launch_description = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(
-                ghost_sim_share_dir,
-                "launch",
-                "joystick.launch.py"
-            )
-        ),
-        condition=launch.conditions.IfCondition(LaunchConfiguration("joystick"))
+    estimator_node = Node(
+        package='ghost_ros',
+        executable='ghost_estimator_node',
+        name='ghost_estimator_node',
+        output='screen',
+        parameters=[ghost_ros_base_dir + "/config/ghost_estimator_config.yaml"]
     )
 
-    # estimator_node = Node(
-    #     package='ghost_ros',
-    #     executable='ghost_estimator_node',
-    #     name='ghost_estimator_node',
-    #     output='screen',
-    #     parameters=[ghost_ros_base_dir + "/config/ghost_estimator_config.yaml"]
-    # )
-
-    # state_machine_node = Node(
-    #     package='ghost_ros',
-    #     executable='robot_state_machine_node',
-    #     name='ghost_state_machine_node',
-    #     output='screen',
-    #     parameters=[ghost_ros_base_dir + "/config/ghost_state_machine_config.yaml"]
-    # )
+    state_machine_node = Node(
+        package='ghost_ros',
+        executable='robot_state_machine_node',
+        name='ghost_state_machine',
+        output='screen',
+        parameters=[ghost_ros_base_dir + "/config/ghost_state_machine_config.yaml"]
+    )
 
     return LaunchDescription([
-        DeclareLaunchArgument(name='joystick', default_value='false'),
+        DeclareLaunchArgument(name='use_joy', default_value='true'),
+        DeclareLaunchArgument(name='channel_id', default_value='1'),
         DeclareLaunchArgument('sim_gui', default_value='true'),
         DeclareLaunchArgument('verbose', default_value='true'),
         simulation,
         # ground_truth_publisher,
-        # rviz_node,
-        joy_launch_description,
+        rviz_node,
         # estimator_node,
-        # state_machine_node,
+        state_machine_node,
+        # v5_actuator_cmd_publisher,
         OpaqueFunction(function = launch_setup)
     ])