State machine (#22)

* State Machine Started * navigation works when hand writing odom * New State machine because the last one gave me a headache. Also action class added * Revert "New State machine because the last one gave me a headache. Also action class added" This reverts commit 0ad1398. * New State machine because the last one gave me a headache. Also action class added * state machine branch start * State Machine Started * navigation works when hand writing odom * New State machine because the last one gave me a headache. Also action class added * Revert "New State machine because the last one gave me a headache. Also action class added" This reverts commit 0ad1398. * New State machine because the last one gave me a headache. Also action class added * state machine branch start * remove old files * rename state machine file * clean up action class * move on start_LED * added field oriented control * changed from effort to velocity * effort controller added * eod 3-17-24 * completes the course consistently (with help up the ramp) * drive to edge works * added try catch for TOF, and working bridge crossing * updated naming and remved unnecessary files for pull requrest * naming changes and cleaned up loggers * removed comment --------- Co-authored-by: Jacob Dent <[email protected]> Co-authored-by: philip <[email protected]> Co-authored-by: nchan18 <[email protected]>
Gold-Rush-Robotics · Mar 20, 2024 · 7a657db · 7a657db
1 parent 14af17a
commit 7a657db
Show file tree

Hide file tree

Showing 31 changed files with 649 additions and 472 deletions.
diff --git a/src/grr_bringup/config/camera_1_param.yaml b/src/grr_bringup/config/camera_1_param.yaml
diff --git a/src/grr_bringup/config/camera_2_param.yaml b/src/grr_bringup/config/camera_2_param.yaml
diff --git a/src/grr_bringup/config/camera_info.yaml b/src/grr_bringup/config/camera_info.yaml
diff --git a/src/grr_bringup/config/controllers.yaml b/src/grr_bringup/config/controllers.yaml
@@ -7,10 +7,9 @@ controller_manager:
 
     grr_cmake_controller:
       type: grr_cmake_controller/MecanumController
-
-    joint_trajectory_controller:
-      type: joint_trajectory_controller/JointTrajectoryController
-      publish_rate: 10  # Hz
+
+    effort_controller:
+      type: effort_controllers/JointGroupEffortController
 
 joint_state_broadcaster:
   ros__parameters:
@@ -65,15 +64,9 @@ grr_cmake_controller:
 
     cmd_vel_timeout: 0.5
     use_stamped_vel: false
-joint_trajectory_controller:
+effort_controller:
   ros__parameters:
+    cmd_timeout: 1.0
     joints:
       - small_package_grabber_roller_1_joint
       - small_package_sweeper_roller_joint
-    #allow messages without all joints
-    allow_partial_joints_goal: true
-    state_interfaces:
-      - position
-      - velocity
-    command_interfaces:
-      - effort
diff --git a/src/grr_bringup/config/servo_params.yaml b/src/grr_bringup/config/servo_params.yaml
@@ -4,22 +4,22 @@ grr/robot:
     pca_frequency: 100
     servo_mapping:
       bridge_latch_joint:
-        port: 15
+        port: 3
         maximum: 4850
         minimum: 3500
       mechanism_lift_joint:
         port: 14
-        maximum: 0
-        minimum: 0
+        maximum: 16435
+        minimum: 10000
       mechanism_package_joint:
         port: 13
-        maximum: 11316
-        minimum: 6880
+        maximum: 10800
+        minimum: 6000
       mechanism_thruster_joint:
         port: 12
         maximum: 11500
         minimum: 3000
       small_package_sweeper_joint:
-        port: 11
-        maximum: 0
-        minimum: 0
+        port: 15
+        maximum: 10500
+        minimum: 2000
diff --git a/src/grr_bringup/launch/isaac.launch.py b/src/grr_bringup/launch/isaac.launch.py
diff --git a/src/grr_bringup/launch/motor_controllers.launch.py b/src/grr_bringup/launch/motor_controllers.launch.py
@@ -15,18 +15,16 @@ def generate_launch_description():
     joystick_file = os.path.join(config_pkg_path, 'config', 'xbox-holonomic.config.yaml')
     xacro_file = os.path.join(description_pkg_path, 'urdf', 'robots','bloodstone.urdf.xacro')
     controllers_file = os.path.join(config_pkg_path, 'config', 'controllers.yaml')
-    print(controllers_file)
     rviz_file = os.path.join(config_pkg_path, 'config', 'config.rviz')
     robot_description_config = xacro.process_file(xacro_file)
     robot_description_xml = robot_description_config.toxml()
-    print(robot_description_xml)
     source_code_path = os.path.abspath(os.path.join(description_pkg_path, "../../../../src/Robot2023/src/grr_description"))
-    urdf_save_path = os.path.join(source_code_path, "bloodstone.urdf")
+    # urdf_save_path = os.path.join(source_code_path, "bloodstone.urdf")
 
-    with open(urdf_save_path, 'w') as f:
-        f.write(robot_description_xml)
+    # with open(urdf_save_path, 'w') as f:
+    #     f.write(robot_description_xml)
     # Create a robot_state_publisher node
-    description_params = {'robot_description': robot_description_xml, 'use_sim_time': True }
+    description_params = {'robot_description': robot_description_xml, 'use_sim_time': False }
     node_robot_state_publisher = Node(
         package='robot_state_publisher',
         executable='robot_state_publisher',
@@ -39,7 +37,7 @@ def generate_launch_description():
     control_node = Node(
         package="controller_manager",
         executable="ros2_control_node",
-        parameters=[{'robot_description': robot_description_xml, 'use_sim_time': True }, controllers_file],
+        parameters=[{'robot_description': robot_description_xml, 'use_sim_time': False }, controllers_file],
         output="screen",
     )
 
@@ -65,23 +63,23 @@ def generate_launch_description():
             on_exit=[mecanum_drive_controller_spawner],
         )
     )
-
+    effort_controllers_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["effort_controller", "--controller-manager", "/controller_manager"],
+        remappings={('/effort_controller/commands', '/dumb_motors')}
+    )
 
     # Start Rviz2 with basic view
     run_rviz2_node = Node(
         package='rviz2',
         executable='rviz2',
-        parameters=[{ 'use_sim_time': True }],
+        parameters=[{ 'use_sim_time': False }],
         name='isaac_rviz2',
         output='screen',
         arguments=[["-d"], [rviz_file]],
     )
 
-
-    # run_rviz2 = ExecuteProcess(
-    #     cmd=['rviz2', '-d', rviz_file],
-    #     output='screen'
-    # )
     rviz2_delay = RegisterEventHandler(
         event_handler=OnProcessExit(
             target_action=joint_state_broadcaster_spawner,
@@ -118,7 +116,8 @@ def generate_launch_description():
         node_robot_state_publisher,
         joint_state_broadcaster_spawner,
         mecanum_drive_controller_delay,
+        effort_controllers_spawner,
         # rviz2_delay,
-        joy,
-        joy_teleop
+        # joy,
+        # joy_teleop
     ])
diff --git a/src/grr_cmake_controller/include/grr_cmake_controller/grr_cmake_controller.hpp b/src/grr_cmake_controller/include/grr_cmake_controller/grr_cmake_controller.hpp
@@ -101,15 +101,16 @@ class MecanumController : public controller_interface::ControllerInterface
 
 protected:
   std::shared_ptr<Wheel> get_wheel(const std::string & wheel_name);
-  std::shared_ptr<Wheel> front_left_handle_;
-  std::shared_ptr<Wheel> front_right_handle_;
-  std::shared_ptr<Wheel> rear_left_handle_;
-  std::shared_ptr<Wheel> rear_right_handle_;
-  std::string front_left_joint_name_;
-  std::string front_right_joint_name_;
-  std::string rear_left_joint_name_;
-  std::string rear_right_joint_name_;
+  std::shared_ptr<Wheel> front_left_wheel_interface_handle_;
+  std::shared_ptr<Wheel> front_right_wheel_interface_handle_;
+  std::shared_ptr<Wheel> rear_left_wheel_interface_handle_;
+  std::shared_ptr<Wheel> rear_right_wheel_interface_handle_;
+  std::string front_left_wheel_joint_name_;
+  std::string front_right_wheel_joint_name_;
+  std::string rear_left_wheel_joint_name_;
+  std::string rear_right_wheel_joint_name_;
   std::shared_ptr<Odometry> odom = std::make_shared<Odometry>();
+  std::shared_ptr<bool> fieldOrientationEnabled = std::make_shared<bool>(true);
 
   struct WheelParams
   {