Dummy launch and services for simulation

autonomy_manager/launch/dummy.launch

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Static TF: base_link -> gq7_link -->
+  <node pkg="tf2_ros" type="static_transform_publisher" name="base_to_odom"
+        args="0 0 0 0 0 0 1 base_link utm_odom" />
+
+  <rosparam param="arm/is_arm_in_home_pose">true</rosparam>
+  <rosparam param="start_utm_x">0</rosparam>
+  <rosparam param="start_utm_y">0</rosparam>
+
+  <!-- <node name="arm_control_node" pkg="autonomy_manager" type="dummy_node.py" output="screen" /> -->
+
+  <node name="utm_to_odom_node" pkg="autonomy_manager" type="dummy_odom_llh.py" output="screen" />
+
+  <node name="dummy_move_base_node" pkg="autonomy_manager" type="dummy_move_base.py" output="screen" />
+
+  <node name="arm_control_node" pkg="autonomy_manager" type="dummy_arm_control.py" output="screen" />
+
+  <node name="pxrf_handler" pkg="autonomy_manager" type="dummy_scan_node.py" output="screen" />
+
+</launch>

autonomy_manager/src/dummy_arm_control.py

@@ -0,0 +1,23 @@
+#!/usr/bin/env python
+import rospy
+from std_srvs.srv import Trigger, TriggerResponse, SetBool, SetBoolResponse
+
+def handle_dummy_service(req):
+    print("Arm Lower service called")
+    return SetBoolResponse(
+        success=True,
+        message="Dummy service response: Service called successfully!"
+    )
+
+def dummy_service_server():
+    rospy.init_node('dummy_service_server')
+
+    lower_arm_service_name = rospy.get_param("lower_arm_service_name")
+
+
+    s = rospy.Service(lower_arm_service_name, SetBool, handle_dummy_service)
+    print("Ready to respond to dummy service calls.")
+    rospy.spin()
+
+if __name__ == "__main__":
+    dummy_service_server()

autonomy_manager/src/dummy_move_base.py

@@ -0,0 +1,35 @@
+#!/usr/bin/env python
+import rospy
+import actionlib
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseResult
+
+class DummyMoveBaseActionServer:
+    def __init__(self):
+        # Initialize the node
+        rospy.init_node('dummy_move_base_action_server')
+        # Create the action server
+        self.server = actionlib.SimpleActionServer('move_base', MoveBaseAction, self.execute, False)
+        # Start the action server
+        self.server.start()
+        rospy.loginfo("Dummy Move Base Action Server has been started.")
+
+    def execute(self, goal):
+        rospy.loginfo("A goal has been received.")
+        rospy.loginfo("Pretending to move the base to the goal...")
+
+        # Wait for a bit to simulate action (not necessary in a real scenario)
+        rospy.sleep(2)
+
+        # Set the result of the action to success
+        result = MoveBaseResult()
+        rospy.loginfo("Reached the goal!")
+        self.server.set_succeeded(result)
+
+if __name__ == '__main__':
+    try:
+        # Instantiate the action server
+        DummyMoveBaseActionServer()
+        # Keep the node alive
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        pass

autonomy_manager/src/dummy_node.py

@@ -0,0 +1,11 @@
+#!/usr/bin/env python3
+
+import rospy
+
+
+if __name__ == "__main__":
+    # Initialize the ROS node
+    rospy.init_node("dummy_node")
+
+    rospy.spin()
+    rospy.loginfo("Shutting down the ROS node.")

autonomy_manager/src/dummy_odom_llh.py

@@ -0,0 +1,36 @@
+#!/usr/bin/env python3
+
+import rospy
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import NavSatFix
+
+if __name__ == "__main__":
+    # Initialize the ROS node
+    rospy.init_node("utm_to_odom_node")
+
+    tf_utm_odom_frame = rospy.get_param("tf_utm_odom_frame")
+    tf_base_link_frame = rospy.get_param("tf_base_link_frame")
+    gps_odom_topic = rospy.get_param("gps_odom_topic")
+    gq7_ekf_llh_topic = rospy.get_param("gq7_ekf_llh_topic")
+
+    utm_odom_pub = rospy.Publisher(gps_odom_topic, Odometry, queue_size=10)
+    ekf_ll_pub = rospy.Publisher(gq7_ekf_llh_topic, NavSatFix, queue_size=10)
+
+
+    # Odom Msg
+    odom_msg = Odometry()
+    odom_msg.header.frame_id = tf_utm_odom_frame
+    odom_msg.child_frame_id = tf_base_link_frame
+    odom_msg.pose.pose.orientation.w = 1
+
+    # Nav Msg    
+    nav_msg = NavSatFix()
+
+    while not rospy.is_shutdown():
+        utm_odom_pub.publish(odom_msg)
+        ekf_ll_pub.publish(nav_msg)
+
+        rospy.sleep(1)
+
+    rospy.spin()
+    rospy.loginfo("Shutting down the ROS node.")

autonomy_manager/src/dummy_scan_node.py

@@ -0,0 +1,20 @@
+#!/usr/bin/env python
+import rospy
+from autonomy_manager.srv import Complete
+
+def handle_dummy_service(req):
+    print("Scan service called")
+    return True
+
+def dummy_service_server():
+    rospy.init_node('dummy_service_server')
+
+    start_scan_service_name = rospy.get_param("start_scan_service_name")
+
+
+    s = rospy.Service(start_scan_service_name, Complete, handle_dummy_service)
+    rospy.loginfo("Ready to respond to dummy scan service calls.")
+    rospy.spin()
+
+if __name__ == "__main__":
+    dummy_service_server()