Autonomy Paramaters

.gitignore

@@ -1,3 +1,5 @@
+vcpkg
+
 teensy_loader_cli
 # datasets
 data/

install-non-ros-deps.sh

@@ -1,6 +1,6 @@
 #!/bin/bash
-apt-get update  # To get the latest package lists
-apt-get install libusb-dev libtool dh-autoreconf libudev-dev ninja-build -y
+sudo apt-get update  # To get the latest package lists
+sudo apt-get install libusb-dev libtool dh-autoreconf libudev-dev ninja-build -y
 
 if [ $(arch) == 'aarch64' ]; then
     if [ -f /etc/nv_tegra_release ]; then

lunabot_bringup/launch/computer.launch

@@ -5,12 +5,11 @@
     <arg name="foxglove" default="false"  />
 
     <rosparam file="$(find lunabot_config)/config/real_robot.yml" command="load" />
+    <rosparam param="autonomy" subst_value="True">$(arg autonomy)</rosparam>
 
     <include if="$(arg foxglove)" file="$(find foxglove_bridge)/launch/foxglove_bridge.launch" />
     <include if="$(arg overhead)" file="$(find lunabot_bringup)/launch/overhead.launch" />
-    <include file="$(find lunabot_control)/launch/manual_control.launch">
-        <arg name="autonomy" value="$(arg autonomy)"/>
-    </include>
+    <include file="$(find lunabot_control)/launch/manual_control.launch"/>
     <include file="$(find lunabot_description)/launch/display.launch"/>
 
 </launch>

lunabot_bringup/launch/robot.launch

@@ -1,6 +1,5 @@
 <?xml version="1.0"?>
 <launch>
-    <arg name="autonomy" default="false" />
     <arg name="record" default="true" />
     <arg name="teensy" default="true" />
     <arg name="cameras" default="true" />
@@ -16,35 +15,32 @@
         <arg name="exp_name" value="$(arg exp_name)"/>
     </include>
 
-    <group if="$(arg autonomy)">
 
-      <include if="$(arg slam)" file="$(find rtabmap_ros)/launch/rtabmap.launch" >
-        <arg name="rtabmap_args" value="--delete_db_on_start"/>
-        <arg name="depth_topic" value="/d455_back/camera/aligned_depth_to_color/image_raw"/>
-        <arg name="rgb_topic" value="/d455_back/camera/color/image_rect_color"/>
-        <arg name="camera_info_topic" value="/d455_back/camera/color/camera_info"/>
-        <arg name="rtabmapviz" value="false" />
-        <arg name="imu_topic" value="/imu" />
-        <arg name="frame_id" value="base_link" />
-      </include>
-
-      <node pkg="lunabot_control" type="effort_factory.py" name="effort_factory" output="screen"/>
-
-      <node pkg="lunabot_control" type="differential_drive_controller.py" name="differential_drive_controller" output="screen"/>
+    <include if="$(arg slam)" file="$(find rtabmap_ros)/launch/rtabmap.launch" >
+      <arg name="rtabmap_args" value="--delete_db_on_start"/>
+      <arg name="depth_topic" value="/d455_back/camera/aligned_depth_to_color/image_raw"/>
+      <arg name="rgb_topic" value="/d455_back/camera/color/image_rect_color"/>
+      <arg name="camera_info_topic" value="/d455_back/camera/color/camera_info"/>
+      <arg name="rtabmapviz" value="false" />
+      <arg name="imu_topic" value="/imu" />
+      <arg name="frame_id" value="base_link" />
+    </include>
 
-      <group ns="nav">
-          <node pkg="lunabot_control" type="mpc_node" name="mpc_node" output="screen"/>
+    <node pkg="lunabot_control" type="effort_factory.py" name="effort_factory" output="screen"/>
 
-          <node name="global_planner_node" pkg="lunabot_nav" type="dstar_node.py" output="screen" respawn="true">
-              <!-- <rosparam file="$(find lunabot_config)/config/rrtstar.yml" command="load"/> -->
-          </node>
-      </group>
+    <node pkg="lunabot_control" type="differential_drive_controller.py" name="differential_drive_controller" output="screen"/>
 
-      <group ns="maps">
-          <node name="costmap_node" pkg="lunabot_perception" type="costmap_node" output="screen">
-              <rosparam file="$(find lunabot_config)/config/global_costmap.yml" command="load" ns="global_costmap" />
-          </node>
-      </group>
+    <group ns="nav">
+      <node pkg="lunabot_control" type="mpc_node" name="mpc_node" output="screen"/>
+      <node name="global_planner_node" pkg="lunabot_nav" type="dstar_node.py" output="screen" respawn="true">
+        <!-- <rosparam file="$(find lunabot_config)/config/rrtstar.yml" command="load"/> -->
+      </node>
+    </group>
 
+    <group ns="maps">
+      <node name="costmap_node" pkg="lunabot_perception" type="costmap_node" output="screen">
+        <rosparam file="$(find lunabot_config)/config/global_costmap.yml" command="load" ns="global_costmap" />
+      </node>
     </group>
+
 </launch>

lunabot_control/launch/control.launch

@@ -1,11 +1,7 @@
 <?xml version="1.0"?>
 <launch>
-  <arg name="autonomy" default="false"/>
 
-
-  <group if="$(arg autonomy)">
-    <node pkg="lunabot_control" type="differential_drive_controller.py" name="diff_drive_controller" output="screen"/>
-    <node pkg="lunabot_control" type="mpc_node" name="mpc_node" output="screen"/>
-  </group>
+  <node pkg="lunabot_control" type="differential_drive_controller.py" name="diff_drive_controller" output="screen"/>
+  <node pkg="lunabot_control" type="mpc_node" name="mpc_node" output="screen"/>
 
 </launch>

lunabot_control/launch/manual_control.launch

@@ -1,11 +1,8 @@
 <?xml version="1.0"?>
 <launch>
-  <arg name="autonomy" default="false" />
+  <node pkg="lunabot_control" type="manual_controller.py" name="manual_control" output="screen"/>
 
-  <node unless="$(arg autonomy)" pkg="lunabot_control" type="manual_controller.py" name="manual_control" output="screen"/>
-  <node if="$(arg autonomy)" pkg="lunabot_control" type="manual_controller_autonomy.py" name="manual_control_autonomy" output="screen"/>
-
- <!-- joy node -->
+  <!-- joy node -->
   <node respawn="true" pkg="joy" type="joy_node" name="joy" output="screen">
     <param name="dev" type="string" value="/dev/input/js0" />
   </node>

lunabot_control/scripts/differential_drive_controller.py

@@ -46,7 +46,8 @@ def __init__(self):
         rospy.on_shutdown(self.shutdown_hook)
 
         while not rospy.is_shutdown():
-            self._loop()
+            if (rospy.get_param("autonomy")):
+                self._loop()
             rate.sleep()
 
     def _vel_cb(self, vel_msg):

lunabot_control/scripts/effort_factory.py

@@ -80,8 +80,9 @@ def stop(self):
     effort_factory = EffortFactory()
 
     while not rospy.is_shutdown():
-        if effort_factory.robot_errors.manual_stop == False:
-            effort_factory.publish_effort()
-        else:
-            effort_factory.stop()
+        if rospy.get_param("autonomy"):
+            if effort_factory.robot_errors.manual_stop == False:
+                effort_factory.publish_effort()
+            else:
+                effort_factory.stop()
         effort_factory.rate.sleep()

lunabot_control/scripts/manual_controller.py

@@ -240,5 +240,6 @@ def stop(self):
     rate = rospy.Rate(20)
 
     while not rospy.is_shutdown():
-        man_ctrl.loop()
+        if not rospy.get_param("autonomy"):
+            man_ctrl.loop()
         rate.sleep()