Merge pull request #108 from open-rdc/caster_controller

add caster_controller

simulator/simulator/launch/gazebo_simulator_active_caster.launch.py

@@ -73,16 +73,25 @@ def generate_launch_description():
         output='screen'
     )
 
+    controller_manager = Node(
+        package='controller_manager',
+        executable='spawner.py',
+        arguments=['position_controller'],
+        output='screen'
+    )
+
+    caster_controller_sim = Node(
+        package='simulator',
+        executable='caster_controller_sim',
+        output='screen'
+    )
+
     return LaunchDescription([
         *launch_args,
         tf_static_publisher,
         gzserver,
         gzclient,
         set_use_sim_time,
-        Node(
-            package='controller_manager',
-            executable='spawner.py',
-            arguments=['position_controller'],
-            output='screen'
-        )
+        controller_manager,
+        caster_controller_sim
     ])

simulator/simulator/models/ai_car_active_caster/model.sdf

@@ -434,7 +434,7 @@
     </plugin>
 
     <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
-      <parameters>/home/haya/ros2_ws/src/AIFormula_private/simulator/simulator/models/ai_car_active_caster/controller_position.yaml</parameters>
+      <parameters>/home/mirai/ros2_ws/src/aiformula/simulator/simulator/models/ai_car_active_caster/controller_position.yaml</parameters>
     </plugin>
 
     <plugin name="turtlebot3_joint_state" filename="libgazebo_ros_joint_state_publisher.so">

simulator/simulator/package.xml

@@ -9,6 +9,12 @@
 
   <depend>common_python</depend>
   <depend>vehicle</depend>
+  <depend>gazebo_ros</depend>
+  <depend>xacro</depend>
+  <depend>gazebo_ros2_control</depend>
+  <depend>joint_state_publisher</depend>
+  <depend>gazebo_ros_pkgs</depend>
+  <depend>ros2_controllers</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

simulator/simulator/setup.py

@@ -17,6 +17,7 @@
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
+            'caster_controller_sim = simulator.caster_controller_sim:main',
         ],
     },
 )

simulator/simulator/simulator/caster_controller_sim.py

@@ -0,0 +1,35 @@
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+from std_msgs.msg import Float64MultiArray
+import math
+
+class CasterControllerSim(Node):
+    def __init__(self):
+        super().__init__("caster_controller_sim_node")
+        self.wheelbase = 0.65
+        self.create_subscription(Twist, "/cmd_vel", self.callback_cmd_vel, 1)
+        self.publisher = self.create_publisher(Float64MultiArray, '/position_controller/commands', 1)
+        self.get_logger().info("caster_controller_sim_node has been started.")
+
+    def callback_cmd_vel(self, msg):
+        pub_msg = Float64MultiArray()
+        if msg.linear.x == 0:
+            pub_msg.data = [0.0]
+        else:
+            print("self.wheelbase: " + str(self.wheelbase))
+            print("msg.angular.z: " + str(msg.angular.z))
+            print("msg.linear.x: " + str(msg.linear.x))
+            print("self.wheelbase * msg.angular.z / msg.linear.x: " + str(self.wheelbase * msg.angular.z / msg.linear.x))
+            pub_msg.data = [-math.asin(max(min(self.wheelbase * msg.angular.z / msg.linear.x, 1.0), -1.0))]
+        self.publisher.publish(pub_msg)
+
+def main():
+    rclpy.init()
+    node = CasterControllerSim()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()