steer with trajectory control

element-robotics · Jun 29, 2022 · 206612d · 206612d
1 parent 1c4b7f0
commit 206612d
Show file tree

Hide file tree

Showing 7 changed files with 1,822 additions and 43 deletions.
diff --git a/mars_rover/config/mars_rover_control.yaml b/mars_rover/config/mars_rover_control.yaml
@@ -12,10 +12,10 @@ controller_manager:
       type: velocity_controllers/JointGroupVelocityController
 
     steer_position_controller:
-      type: position_controllers/JointGroupPositionController
+      type: joint_trajectory_controller/JointTrajectoryController
 
     wheel_tree_position_controller:
-      type: position_controllers/JointGroupPositionController
+      type: effort_controllers/JointGroupEffortController
 
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
@@ -69,6 +69,11 @@ steer_position_controller:
       - suspension_steer_F_R_joint
       - suspension_steer_B_R_joint
     interface_name: position
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
 
 wheel_tree_position_controller:
   ros__parameters:
@@ -79,4 +84,9 @@ wheel_tree_position_controller:
       - suspension_arm_F_R_joint
       - suspension_arm_B_R_joint
       - suspension_arm_B2_R_joint
-    interface_name: position
+    command_interfaces:
+      - effort
+    state_interfaces:
+      - position
+      - velocity
+      - effort
diff --git a/mars_rover/launch/mars_rover.launch.py b/mars_rover/launch/mars_rover.launch.py
@@ -8,6 +8,10 @@
 import os
 from os import environ
 
+from ament_index_python.packages import get_package_share_directory
+
+import xacro
+
 def generate_launch_description():
     # ld = LaunchDescription()
 
@@ -16,39 +20,33 @@ def generate_launch_description():
                      environ.get('LD_LIBRARY_PATH', default='')])}
 
 
-    urdf_model_path = os.path.join(FindPackageShare(package='mars_rover').find('mars_rover'), 'urdf/curiosity_mars_rover.xacro')
+    mars_rover_demos_path = os.path.join(
+        get_package_share_directory('mars_rover'))
+
+    urdf_model_path = os.path.join(mars_rover_demos_path, 'urdf/curiosity_mars_rover.xacro.urdf')
     mars_world_model = os.path.join(FindPackageShare(package='mars_rover').find('mars_rover'), 'worlds/mars_curiosity.world')
 
-    model_arg = DeclareLaunchArgument(
-        name="model", 
-        default_value=urdf_model_path,
-        description="mars rover urdf"
-    )
+
+    doc = xacro.parse(open(urdf_model_path))
+    xacro.process_doc(doc)
+    params = {'robot_description': doc.toxml()}
+
 
     arm_node = Node(
         package="mars_rover",
         executable="move_arm",
-        parameters=[
-            {"robot_description": Command(['xacro ', LaunchConfiguration('model')])}
-        ],
         output='screen'
     )
 
     mast_node = Node(
         package="mars_rover",
         executable="move_mast",
-        parameters=[
-            {"robot_description": Command(['xacro ', LaunchConfiguration('model')])}
-        ],
         output='screen'
     )
 
     wheel_node = Node(
         package="mars_rover",
         executable="move_wheel",
-        parameters=[
-            {"robot_description": Command(['xacro ', LaunchConfiguration('model')])}
-        ],
         output='screen'
     )
 
@@ -65,7 +63,7 @@ def generate_launch_description():
         shell=True
     )
 
-    params = {'use_sim_time': True, 'robot_description': Command(['xacro ', LaunchConfiguration('model')])}
+
     robot_state_publisher = Node(
             package='robot_state_publisher',
             executable='robot_state_publisher',
@@ -81,7 +79,8 @@ def generate_launch_description():
             '-x','1.0',
             '-z','-7.4',
             '-y','0.0',
-            '-topic', '/robot_description'
+            '-string', doc.toxml(),
+            '-allow_renaming', 'true'
         ],
         output='screen'
     )
@@ -139,7 +138,6 @@ def generate_launch_description():
 
 
     return LaunchDescription([
-        model_arg,
         start_world,
         robot_state_publisher,
         spawn,
@@ -163,8 +161,13 @@ def generate_launch_description():
         RegisterEventHandler(
             OnProcessExit(
                 target_action=set_hardware_interface_active,
-                on_exit=[load_joint_state_broadcaster,
-                        load_arm_joint_traj_controller,
+                on_exit=[load_joint_state_broadcaster],
+            )
+        ),
+        RegisterEventHandler(
+            OnProcessExit(
+                target_action=load_joint_state_broadcaster,
+                on_exit=[load_arm_joint_traj_controller,
                         load_wheel_joint_traj_controller,
                         load_steer_joint_traj_controller,
                         load_suspension_joint_traj_controller],

diff --git a/mars_rover/nodes/move_arm b/mars_rover/nodes/move_arm
@@ -19,6 +19,7 @@ class MoveArm(Node):
 
         self.open = True
 
+
     def open_arm_callback(self, request, response):
         self.open = True
         traj = JointTrajectory()
@@ -32,6 +33,7 @@ class MoveArm(Node):
         traj.points.append(point1)
         self.arm_publisher_.publish(traj)
 
+
         return response
 
     def close_arm_callback(self, request, response):

diff --git a/mars_rover/nodes/move_wheel b/mars_rover/nodes/move_wheel
@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 
+from pickle import FALSE
 import rclpy
 from rclpy.node import Node
 from builtin_interfaces.msg import Duration
@@ -14,16 +15,24 @@ class MoveWheel(Node):
     def __init__(self):
         super().__init__('wheel_node')
         self.wheel_publisher_ = self.create_publisher(Float64MultiArray, '/wheel_velocity_controller/commands', 10)
-        self.steer_publisher_ = self.create_publisher(Float64MultiArray, '/steer_position_controller/commands', 10)
+        self.steer_publisher_ = self.create_publisher(JointTrajectory, '/steer_position_controller/joint_trajectory', 10)
         self.suspension_publisher_ = self.create_publisher(Float64MultiArray, '/wheel_tree_position_controller/commands',10)
         timer_period = 0.1  # seconds
         self.timer = self.create_timer(timer_period, self.timer_callback)
 
 
         self.vel_sub = self.create_subscription(Twist, '/cmd_vel', self.vel_callback, 10)
         self.curr_vel = Twist()
+        self.last_vel = Twist()
+        self.should_steer = False
 
     def vel_callback(self, msg):
+        if abs(self.last_vel.angular.z - self.curr_vel.angular.z) > 0.01 and self.should_steer is False:
+            self.last_vel = Twist()
+            self.last_vel.linear.x = self.curr_vel.linear.x
+            self.last_vel.angular.z = self.curr_vel.angular.z
+            self.should_steer = True
+
         self.curr_vel = msg
 
     def set_wheel_common_speed(self, vel):
@@ -46,9 +55,14 @@ class MoveWheel(Node):
 
     def set_steering(self, turn_rad):
         #Ideal Ackerman Steering
-        target_steer = Float64MultiArray()
+        target_steer = JointTrajectory()
+        # target_steer.header.stamp = self.get_clock().now().to_msg()
+        target_steer.joint_names = ["suspension_steer_F_L_joint", "suspension_steer_B_L_joint", 
+                                    "suspension_steer_F_R_joint", "suspension_steer_B_R_joint"]
+        steer_point = JointTrajectoryPoint()
+
         if abs(turn_rad) < 1e-3:
-            target_steer.data = [0.0, 0.0, 0.0, 0.0]
+            steer_point.positions = [0.0, 0.0, 0.0, 0.0]
         else:
             R = abs(turn_rad)         #Turning radius
 
@@ -64,10 +78,18 @@ class MoveWheel(Node):
             if alpha_o > 0.6:
                 alpha_o = 0.6
 
+            alpha_i = round(alpha_i, 2)
+            alpha_o = round(alpha_o, 2)
+
             if turn_rad > 0.0:
-                target_steer.data = [alpha_i, -alpha_i, alpha_o, -alpha_o]
+                steer_point.positions = [alpha_i, -alpha_i, alpha_o, -alpha_o]
             else:
-                target_steer.data = [-alpha_o, alpha_o, -alpha_i, alpha_i]
+                steer_point.positions = [-alpha_o, alpha_o, -alpha_i, alpha_i]
+
+
+        steer_point.time_from_start = Duration(sec=1)
+
+        target_steer.points.append(steer_point)
 
         self.steer_publisher_.publish(target_steer)
 
@@ -76,16 +98,18 @@ class MoveWheel(Node):
 
         target_val = Float64MultiArray()
         if sus_val is None:
-            target_val.data = [0.0,0.0,0.0,0.0,0.0,0.0]
+            target_val.data = [600.0,-700.0,-100.0,600.0,-700.0,-100.0]
         self.suspension_publisher_.publish(target_val)
 
 
     def timer_callback(self):
         self.set_wheel_common_speed(self.curr_vel.linear.x)
         self.set_suspension()
 
-        steering_val = self.map_angular_to_steering(self.curr_vel.angular.z)
-        self.set_steering(steering_val)
+        if self.should_steer:
+            steering_val = self.map_angular_to_steering(self.curr_vel.angular.z)
+            self.set_steering(steering_val)
+            self.should_steer = False
 
 
 def main(args=None):

diff --git a/mars_rover/package.xml b/mars_rover/package.xml
@@ -13,6 +13,24 @@
   <depend>rclcpp</depend>
   <depend>rclpy</depend>
 
+  <exec_depend>ament_index_python</exec_depend>
+  <exec_depend>control_msgs</exec_depend>
+  <exec_depend>effort_controllers</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>hardware_interface</exec_depend>
+  <exec_depend>ign_ros2_control</exec_depend>
+  <exec_depend>imu_sensor_broadcaster</exec_depend>
+  <exec_depend>joint_state_broadcaster</exec_depend>
+  <exec_depend>joint_trajectory_controller</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>ros_ign_gazebo</exec_depend>
+  <exec_depend>ros2controlcli</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>velocity_controllers</exec_depend>
+  <exec_depend>diff_drive_controller</exec_depend>
+  <exec_depend>xacro</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

diff --git a/mars_rover/urdf/curiosity_mars_rover.xacro b/mars_rover/urdf/curiosity_mars_rover.xacro
@@ -96,43 +96,59 @@
         <joint name="suspension_steer_F_L_joint">
             <command_interface name="position" />
             <state_interface name="position" />
+            <state_interface name="velocity" />
         </joint>
         <joint name="suspension_steer_B_L_joint">
             <command_interface name="position" />
             <state_interface name="position" />
+            <state_interface name="velocity" />
         </joint>
         <joint name="suspension_steer_F_R_joint">
             <command_interface name="position" />
             <state_interface name="position" />
+            <state_interface name="velocity" />
         </joint>
         <joint name="suspension_steer_B_R_joint">
             <command_interface name="position" />
             <state_interface name="position" />
+            <state_interface name="velocity" />
         </joint>
         <!-- Suspension Arms Interfaces-->
         <joint name="suspension_arm_F_L_joint">
-            <command_interface name="position" />
-            <state_interface name="position" />
+            <command_interface name="effort"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
         </joint>
         <joint name="suspension_arm_B_L_joint">
-            <command_interface name="position" />
-            <state_interface name="position" />
+            <command_interface name="effort"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
         </joint>
         <joint name="suspension_arm_B2_L_joint">
-            <command_interface name="position" />
-            <state_interface name="position" />
+            <command_interface name="effort"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
         </joint>
         <joint name="suspension_arm_F_R_joint">
-            <command_interface name="position" />
-            <state_interface name="position" />
+            <command_interface name="effort"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
         </joint>
         <joint name="suspension_arm_B_R_joint">
-            <command_interface name="position" />
-            <state_interface name="position" />
+            <command_interface name="effort"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
         </joint>
         <joint name="suspension_arm_B2_R_joint">
-            <command_interface name="position" />
-            <state_interface name="position" />
+            <command_interface name="effort"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
         </joint>
     </ros2_control>