added demo service

mars_rover/CMakeLists.txt

@@ -46,6 +46,7 @@ install(PROGRAMS
   nodes/move_arm
   nodes/move_mast
   nodes/move_wheel
+  nodes/run_demo
   DESTINATION lib/${PROJECT_NAME}
 )
 

mars_rover/launch/mars_rover.launch.py

@@ -52,6 +52,12 @@ def generate_launch_description():
         output='screen'
     )
 
+    run_node = Node(
+        package="mars_rover",
+        executable="run_demo",
+        output='screen'
+    )
+
     start_world = ExecuteProcess(
         cmd=['ign gazebo', mars_world_model, '-r'],
         output='screen',
@@ -140,6 +146,7 @@ def generate_launch_description():
         arm_node,
         mast_node,
         wheel_node,
+        run_node,
 
         RegisterEventHandler(
             OnProcessExit(

mars_rover/nodes/move_arm

@@ -16,15 +16,12 @@ class MoveArm(Node):
         self.pick_srv = self.create_service(Empty, 'move_arm', self.move_arm_callback)
 
 
-        # timer_period = 20.0  # seconds
-        # self.timer = self.create_timer(timer_period, self.timer_callback)
-
     def move_arm_callback(self, request, response):
         traj = JointTrajectory()
         traj.joint_names = ["arm_01_joint", "arm_02_joint", "arm_03_joint", "arm_04_joint", "arm_tools_joint"]
 
         point1 = JointTrajectoryPoint()
-        point1.positions = [0.0,0.0,1.0,0.0,0.0]
+        point1.positions = [-1.57,-0.4,-1.1,-1.57,-1.57]
         point1.time_from_start = Duration(sec=4)
 
         traj.points.append(point1)

mars_rover/nodes/move_wheel

@@ -45,6 +45,7 @@ class MoveWheel(Node):
 
 
     def set_steering(self, turn_rad):
+        #Ideal Ackerman Steering
         target_steer = Float64MultiArray()
         if abs(turn_rad) < 1e-3:
             target_steer.data = [0.0, 0.0, 0.0, 0.0]

mars_rover/nodes/run_demo

@@ -0,0 +1,68 @@
+#!/usr/bin/env python3
+
+
+import rclpy
+from rclpy.node import Node
+
+from geometry_msgs.msg import Twist
+import math
+from random import randint
+from std_srvs.srv import Empty
+
+class RunDemo(Node):
+    def __init__(self) -> None:
+        super().__init__('run_node')
+        self.motion_publisher_ = self.create_publisher(Twist, '/cmd_vel', 10)
+        self.random_srv = self.create_service(Empty, 'random_walk', self.random_walk_callback)
+
+        timer_period = 10
+        self.timer = self.create_timer(timer_period, self.timer_callback)
+
+        action1 = Twist()
+        action1.linear.x = 2.0
+
+        action2 = Twist()
+        action2.linear.x = 3.0
+        action2.angular.z = 0.4
+
+        action3 = Twist()
+        action3.linear.x = 3.0
+        action3.angular.z = -0.4
+
+        self.actions = [action1, action2, action3]
+
+        self.random_walk = False
+
+    def timer_callback(self):
+        if self.random_walk:
+            curr_action = self.actions[randint(0,2)]
+            self.motion_publisher_.publish(curr_action)
+
+    def random_walk_callback(self, request, response):
+        if self.random_walk:
+            self.motion_publisher_.publish(Twist())
+        else:
+            self.motion_publisher_.publish(self.actions[0])
+
+        self.random_walk = not self.random_walk
+        return response
+
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    run_demo = RunDemo()
+
+    rclpy.spin(run_demo)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    run_demo.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

mars_rover/package.xml

@@ -13,6 +13,7 @@
   <depend>rclcpp</depend>
   <depend>rclpy</depend>
 
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>