ackerman steering done

element-robotics · Jun 21, 2022 · 68dbf2e · 68dbf2e
1 parent 8dee9f8
commit 68dbf2e
Show file tree

Hide file tree

Showing 4 changed files with 110 additions and 117 deletions.
diff --git a/mars_rover/config/mars_rover_control.yaml b/mars_rover/config/mars_rover_control.yaml
@@ -9,45 +9,45 @@ controller_manager:
       type: joint_trajectory_controller/JointTrajectoryController
 
     wheel_velocity_controller:
-      type: joint_trajectory_controller/JointTrajectoryController
+      type: velocity_controllers/JointGroupVelocityController
 
     steer_position_controller:
-      type: joint_trajectory_controller/JointTrajectoryController
+      type: position_controllers/JointGroupPositionController
 
     wheel_tree_position_controller:
-      type: joint_trajectory_controller/JointTrajectoryController
+      type: position_controllers/JointGroupPositionController
 
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
 
-arm_joint_trajectory_controller:
-  ros__parameters:
-    joints:
-      - arm_01_joint
-      - arm_02_joint
-      - arm_03_joint
-      - arm_04_joint
-      - arm_tools_joint
-    interface_name: position
-    command_interfaces:
-      - position
-    state_interfaces:
-      - position
-      - velocity
+# arm_joint_trajectory_controller:
+#   ros__parameters:
+#     joints:
+#       - arm_01_joint
+#       - arm_02_joint
+#       - arm_03_joint
+#       - arm_04_joint
+#       - arm_tools_joint
+#     interface_name: position
+#     command_interfaces:
+#       - position
+#     state_interfaces:
+#       - position
+#       - velocity
 
-mast_joint_trajectory_controller:
-  ros__parameters:
-    joints:
-      - mast_p_joint
-      - mast_02_joint
-      - mast_cameras_joint
-    interface_name: position
-    command_interfaces:
-      - position
-    state_interfaces:
-      - position
-      - velocity
+# mast_joint_trajectory_controller:
+#   ros__parameters:
+#     joints:
+#       - mast_p_joint
+#       - mast_02_joint
+#       - mast_cameras_joint
+#     interface_name: position
+#     command_interfaces:
+#       - position
+#     state_interfaces:
+#       - position
+#       - velocity
 
 wheel_velocity_controller:
   ros__parameters:
@@ -59,11 +59,6 @@ wheel_velocity_controller:
       - middle_wheel_R_joint
       - back_wheel_R_joint
     interface_name: velocity
-    command_interfaces:
-      - velocity
-    state_interfaces:
-      - position
-      - velocity
 
 
 steer_position_controller:
@@ -74,11 +69,6 @@ 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:
@@ -90,7 +80,3 @@ wheel_tree_position_controller:
       - suspension_arm_B_R_joint
       - suspension_arm_B2_R_joint
     interface_name: position
-    command_interfaces:
-      - position
-    state_interfaces:
-      - position
diff --git a/mars_rover/launch/mars_rover.launch.py b/mars_rover/launch/mars_rover.launch.py
@@ -73,7 +73,7 @@ def generate_launch_description():
         arguments=[
             '-name', 'curiosity_mars_rover',
             '-x','1.0',
-            '-z','-7.0',
+            '-z','-7.4',
             '-y','0.0',
             '-topic', '/robot_description'
         ],
@@ -99,17 +99,17 @@ def generate_launch_description():
         output='screen'
     )
 
-    load_arm_joint_traj_controller = ExecuteProcess(
-        cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
-             'arm_joint_trajectory_controller'],
-        output='screen'
-    )
+    # load_arm_joint_traj_controller = ExecuteProcess(
+    #     cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
+    #          'arm_joint_trajectory_controller'],
+    #     output='screen'
+    # )
 
-    load_mast_joint_traj_controller = ExecuteProcess(
-        cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
-             'mast_joint_trajectory_controller'],
-        output='screen'
-    )
+    # load_mast_joint_traj_controller = ExecuteProcess(
+    #     cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
+    #          'mast_joint_trajectory_controller'],
+    #     output='screen'
+    # )
 
     load_wheel_joint_traj_controller = ExecuteProcess(
         cmd=['ros2', 'control', 'load_controller', '--set-state', 'start',
@@ -157,8 +157,6 @@ def generate_launch_description():
             OnProcessExit(
                 target_action=set_hardware_interface_active,
                 on_exit=[load_joint_state_broadcaster,
-                        load_arm_joint_traj_controller,
-                        load_mast_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_wheel b/mars_rover/nodes/move_wheel
@@ -4,69 +4,89 @@ import rclpy
 from rclpy.node import Node
 from builtin_interfaces.msg import Duration
 
-from std_msgs.msg import String, Float64
+from std_msgs.msg import String, Float64MultiArray
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from geometry_msgs.msg import Twist
+import math
 
 class MoveWheel(Node):
 
     def __init__(self):
         super().__init__('wheel_node')
-        self.wheel_publisher_ = self.create_publisher(JointTrajectory, '/wheel_velocity_controller/joint_trajectory', 10)
-        self.steer_publisher_ = self.create_publisher(JointTrajectory, '/steer_position_controller/joint_trajectory', 10)
-#        self.suspension_publisher_ = self.create_publisher(JointTrajectory, '/wheel_tree_position_controller/joint_trajectory',10)
-        timer_period = 0.5  # seconds
+        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.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)
 
 
-    def timer_callback(self):
-        #wheel velocities
-        traj = JointTrajectory()
-        traj.joint_names = ["front_wheel_L_joint",
-                            "middle_wheel_L_joint",
-                            "back_wheel_L_joint",
-                            "front_wheel_R_joint",
-                            "middle_wheel_R_joint",
-                            "back_wheel_R_joint"]
-
-        point = JointTrajectoryPoint()
-        point.positions = [0.0,0.0,0.0,0.0,0.0,0.0]
-        point.time_from_start = Duration(sec=1)
+        self.vel_sub = self.create_subscription(Twist, '/cmd_vel', self.vel_callback, 10)
+        self.curr_vel = Twist()
 
-        traj.points.append(point)
-        self.wheel_publisher_.publish(traj)
+    def vel_callback(self, msg):
+        self.curr_vel = msg
 
+    def set_wheel_common_speed(self, vel):
 
-        #steering wheel
-        steer_traj = JointTrajectory()
-        steer_traj.joint_names = ["suspension_steer_F_L_joint",
-                                    "suspension_steer_B_L_joint",
-                                    "suspension_steer_F_R_joint",
-                                    "suspension_steer_B_R_joint"]
+        target_vel = Float64MultiArray()
+        target_vel.data = [vel for i in range(3)] + [-vel for i in range(3,6)]
 
-        steer_point = JointTrajectoryPoint()
-        steer_point.positions = [1.0,1.0,0.0,0.0]
-        steer_point.time_from_start = Duration(sec=1)
+        # self.get_logger().info(f'Publishing: "{target_vel.data}"')
+        self.wheel_publisher_.publish(target_vel)
 
-        steer_traj.points.append(steer_point)
-        self.steer_publisher_.publish(steer_traj)
 
-        # #wheel suspension
-        # suspension_traj = JointTrajectory()
-        # suspension_traj.joint_names = ["suspension_arm_F_L_joint",
-        #                                 "suspension_arm_B_L_joint",
-        #                                 "suspension_arm_B2_L_joint",
-        #                                 "suspension_arm_F_R_joint",
-        #                                 "suspension_arm_B_R_joint",
-        #                                 "suspension_arm_B2_R_joint"]
+    def map_angular_to_steering(self, angular_speed) -> float:
+        if abs(angular_speed) < 1e-3:
+            return 0.0
 
-        # suspension_point = JointTrajectoryPoint()
-        # suspension_point.positions = [0.0,0.0,0.0,0.0,0.0,0.0]
-        # suspension_point.time_from_start = Duration(sec=1)
+        #max 0.6 min -0.6
+        angular_speed = min(0.6, max(angular_speed, -0.6))
+        return (angular_speed/abs(angular_speed))*(-25 * abs(angular_speed) + 17.5)
 
-        # suspension_traj.points.append(suspension_point)
-        # self.suspension_publisher_.publish(suspension_traj)
 
-
+    def set_steering(self, turn_rad):
+        target_steer = Float64MultiArray()
+        if abs(turn_rad) < 1e-3:
+            target_steer.data = [0.0, 0.0, 0.0, 0.0]
+        else:
+            R = abs(turn_rad)         #Turning radius
+
+            L = 2.08157     #Chassis Length
+            T = 1.53774     #Chassis Width
+
+            alpha_i = math.atan(L/(R - (T/2)))
+            alpha_o = math.atan(L/(R + (T/2)))
+
+            if alpha_i > 0.6:
+                alpha_i = 0.6
+
+            if alpha_o > 0.6:
+                alpha_o = 0.6
+
+            if turn_rad > 0.0:
+                target_steer.data = [alpha_i, -alpha_i, alpha_o, -alpha_o]
+            else:
+                target_steer.data = [-alpha_o, alpha_o, -alpha_i, alpha_i]
+
+        self.get_logger().info(f'Publishing: "{target_steer.data}"')
+        self.steer_publisher_.publish(target_steer)
+
+
+    def set_suspension(self, sus_val=None):
+
+        target_val = Float64MultiArray()
+        if sus_val is None:
+            target_val.data = [0.1,0.0,-0.1,0.1,0.0,-0.1]
+        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)
+
 
 def main(args=None):
     rclpy.init(args=args)

diff --git a/mars_rover/urdf/curiosity_mars_rover.xacro b/mars_rover/urdf/curiosity_mars_rover.xacro
@@ -30,6 +30,7 @@
         <hardware>
             <plugin>ign_ros2_control/IgnitionSystem</plugin>
         </hardware>
+        <!-- Arm Joints Interfaces-->
         <joint name="arm_01_joint">
             <command_interface name="position" />
             <state_interface name="position" />
@@ -56,6 +57,7 @@
             <state_interface name="position" />
             <state_interface name="velocity" />
         </joint>
+        <!-- Mast Joints Interfaces-->
         <joint name="mast_p_joint">
             <command_interface name="position" />
             <state_interface name="position" />
@@ -71,56 +73,43 @@
             <state_interface name="position" />
             <state_interface name="velocity" />
         </joint>
+        <!-- Wheel Joints Interfaces-->
         <joint name="front_wheel_L_joint">
             <command_interface name="velocity" />
-            <state_interface name="position" />
-            <state_interface name="velocity" />
         </joint>
         <joint name="middle_wheel_L_joint">
             <command_interface name="velocity" />
-            <state_interface name="position" />
-            <state_interface name="velocity" />
         </joint>
         <joint name="back_wheel_L_joint">
             <command_interface name="velocity" />
-            <state_interface name="position" />
-            <state_interface name="velocity" />
         </joint>
         <joint name="front_wheel_R_joint">
             <command_interface name="velocity" />
-            <state_interface name="position" />
-            <state_interface name="velocity" />
         </joint>
         <joint name="middle_wheel_R_joint">
             <command_interface name="velocity" />
-            <state_interface name="position" />
-            <state_interface name="velocity" />
         </joint>
         <joint name="back_wheel_R_joint">
             <command_interface name="velocity" />
-            <state_interface name="position" />
-            <state_interface name="velocity" />
         </joint>
+        <!-- Steering Joints Interfaces-->
         <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" />