Lander align

CMakeLists.txt

@@ -48,15 +48,19 @@ set(MROVER_ROS_DEPENDENCIES
         tf2
         tf2_ros
         tf2_geometry_msgs
+        actionlib_msgs
 )
 
 extract_filenames(msg/*.msg MROVER_MESSAGE_FILES)
 
 extract_filenames(srv/*.srv MROVER_SERVICE_FILES)
 
+extract_filenames(action/*.action MROVER_ACTION_FILES)
+
 set(MROVER_MESSAGE_DEPENDENCIES
         std_msgs
         sensor_msgs
+        actionlib_msgs
 )
 
 set(MROVER_PARAMETERS
@@ -97,13 +101,15 @@ endforeach ()
 set(BUILD_SHARED_LIBS ON)
 
 ### ======== ###
-### Messages ###
+### Messages & Actions & Reconfigure ###
 ### ======== ###
 
 add_message_files(FILES ${MROVER_MESSAGE_FILES})
 
 add_service_files(FILES ${MROVER_SERVICE_FILES})
 
+add_action_files(DIRECTORY action FILES ${MROVER_ACTION_FILES})
+
 generate_messages(DEPENDENCIES ${MROVER_MESSAGE_DEPENDENCIES})
 
 generate_dynamic_reconfigure_options(${MROVER_PARAMETERS})
@@ -214,7 +220,9 @@ endif ()
 
 ## Teleoperation
 
+mrover_add_nodelet(lander_align src/teleoperation/lander_align/*.cpp src/teleoperation/lander_align src/teleoperation/lander_align/pch.hpp)
 mrover_add_node(arm_controller src/teleoperation/arm_controller/*.cpp)
+mrover_nodelet_link_libraries(lander_align lie)
 
 ## Simulator
 
@@ -251,4 +259,4 @@ catkin_add_nosetests(test/util/SO3_test.py)
 
 install(DIRECTORY launch/
         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
-)
+)

action/LanderAlign.action

@@ -0,0 +1,8 @@
+# Define the goal
+uint32 num  # Specify which dishwasher we want to use
+---
+# Define the result
+uint32 num
+---
+# Define a feedback message
+string curr_state

config/simulator/simulator.yaml

@@ -38,3 +38,7 @@ objects:
     name: rock4
     uri: package://mrover/urdf/world/rock.urdf.xacro
     translation: [9, -2, 0.2]
+  - type: urdf
+    name: lander
+    uri: package://mrover/urdf/world/lander.urdf.xacro
+    translation: [8, 6, 2]

launch/autonomy.launch

@@ -11,7 +11,7 @@
     <arg name="run_object_detector" value="$(arg run_object_detector)" />
   </include>
 
-  <include file="$(find mrover)/launch/navigation.launch" />
+  <!-- <include file="$(find mrover)/launch/navigation.launch" /> -->
 
   <include file="$(find mrover)/launch/localization.launch">
     <arg name="sim" value="$(arg sim)" />

launch/simulator.launch

@@ -3,6 +3,7 @@
   <arg name="run_rviz" default="true"/>
 
   <rosparam command="load" file="$(find mrover)/config/simulator/simulator.yaml"/>
+  <rosparam command="load" file="$(find mrover)/config/localization.yaml" />
 
   <include file="$(find mrover)/launch/autonomy.launch">
     <arg name="sim" value="true"/>

package.xml

@@ -85,5 +85,6 @@
     <nodelet plugin="${prefix}/plugins/can.xml" />
     <nodelet plugin="${prefix}/plugins/simulator.xml" />
     <nodelet plugin="${prefix}/plugins/nv_gst_h265_enc.xml" />
+    <nodelet plugin="${prefix}/plugins/lander_align_plugin.xml" />
   </export>
 </package>

plugins/lander_align_plugin.xml

@@ -0,0 +1,6 @@
+<library path="lib/liblander_align_nodelet">
+    <class name="mrover/LanderAlignNodelet"
+           type="mrover::LanderAlignNodelet"
+           base_class_type="nodelet::Nodelet">
+    </class>
+</library>

src/simulator/shaders/shaders.wgsl

@@ -65,6 +65,7 @@ struct OutFragment {
     let specular = specularStrength * spec * su.lightColor;
     // Combination
     out.color = vec4(((ambient + diffuse + specular) * baseColor).rgb, 1);
+    // out.color = vec4(in.normalInWorld.x, in.normalInWorld.y, in.normalInWorld.z, 1);    
     return out;
 }
 

src/teleoperation/backend/consumers.py

@@ -8,6 +8,7 @@
 import rospy
 import tf2_ros
 import cv2
+import actionlib
 from cv_bridge import CvBridge
 from geometry_msgs.msg import Twist
 from mrover.msg import (
@@ -25,8 +26,10 @@
     Velocity,
     Position,
     IK,
+    LanderAlignAction,
+    LanderAlignGoal
 )
-from mrover.srv import EnableAuton, ChangeCameras, CapturePanorama
+from mrover.srv import EnableAuton, ChangeCameras
 from sensor_msgs.msg import NavSatFix, Temperature, RelativeHumidity, Image
 from mrover.srv import EnableAuton
 from sensor_msgs.msg import NavSatFix, Temperature, RelativeHumidity
@@ -100,7 +103,7 @@ def connect(self):
             self.cache_enable_limit = rospy.ServiceProxy("cache_enable_limit_switches", SetBool)
             self.calibrate_service = rospy.ServiceProxy("arm_calibrate", Trigger)
             self.change_cameras_srv = rospy.ServiceProxy("change_cameras", ChangeCameras)
-            self.capture_panorama_srv: Any = rospy.ServiceProxy("capture_panorama", CapturePanorama)
+            # self.capture_panorama_srv = rospy.ServiceProxy("capture_panorama", CapturePanorama)
 
             # ROS Parameters
             self.mappings = rospy.get_param("teleop/joystick_mappings")
@@ -121,6 +124,9 @@ def connect(self):
             self.flight_thread = threading.Thread(target=self.flight_attitude_listener)
             self.flight_thread.start()
 
+            #Actions Servers
+            self.landerClient = actionlib.SimpleActionClient('LanderAlignAction', LanderAlignAction)
+
         except rospy.ROSException as e:
             rospy.logerr(e)
 
@@ -180,6 +186,10 @@ def receive(self, text_data):
                 self.save_basic_waypoint_list(message)
             elif message["type"] == "get_basic_waypoint_list":
                 self.get_basic_waypoint_list(message)
+            elif message["type"] == "start_lander_align":
+                self.start_lander_align()
+            elif message["type"] == "stop_lander_align":
+                self.stop_lander_align()
         except Exception as e:
             rospy.logerr(e)
 
@@ -711,3 +721,14 @@ def flight_attitude_listener(self):
             self.send(text_data=json.dumps({"type": "flight_attitude", "pitch": pitch, "roll": roll}))
 
             rate.sleep()
+
+    def start_lander_align(self) -> None:
+        goal = LanderAlignGoal()
+        self.landerClient.send_goal(goal)
+        pass
+
+    def stop_lander_align(self) -> None:
+        if(self.landerClient.get_state() != 3):
+            self.landerClient.cancel_goal()
+        pass
+

src/teleoperation/frontend/src/components/AutonTask.vue

@@ -23,6 +23,12 @@
         <p style="margin-top: 6px">Joystick Values</p>
         <JoystickValues />
       </div>
+      <div class="my-5">
+        <ToggleButton :labelEnableText="'Lander Align Enabled'" 
+        :labelDisableText="'Lander Align Disabled'" 
+        :currentState="landerAlignEnabled"
+        @change="toggleLander()"></ToggleButton>
+      </div>
       <OdometryReading :odom="odom" />
     </div>
     <div class="shadow p-3 rounded map">
@@ -68,6 +74,7 @@ import OdometryReading from './OdometryReading.vue'
 import JoystickValues from './JoystickValues.vue'
 import DriveControls from './DriveControls.vue'
 import MastGimbalControls from './MastGimbalControls.vue'
+import ToggleButton from './ToggleButton.vue'
 import { quaternionToMapAngle } from '../utils.js'
 import { defineComponent } from 'vue'
 
@@ -84,7 +91,8 @@ export default defineComponent({
     OdometryReading,
     JoystickValues,
     DriveControls,
-    MastGimbalControls
+    MastGimbalControls,
+    ToggleButton
   },
 
   // add prop where map has the center property and autontask sends it once it gets it
@@ -121,7 +129,9 @@ export default defineComponent({
         effort: [] as number[],
         state: [] as string[],
         error: [] as string[]
-      }
+      },
+
+      landerAlignEnabled: false,
     }
   },
 
@@ -168,7 +178,13 @@ export default defineComponent({
   },
 
   methods: {
-    ...mapActions('websocket', ['sendMessage'])
+    ...mapActions('websocket', ['sendMessage']),
+
+    toggleLander() {
+      this.landerAlignEnabled = !this.landerAlignEnabled;
+      if(this.landerAlignEnabled) this.sendMessage({"type": "start_lander_align"});
+      else this.sendMessage({"type": "stop_lander_align"});
+    }
   },
 
   beforeUnmount: function () {

src/teleoperation/lander_align/ClientDummy.py

@@ -0,0 +1,53 @@
+#!/usr/bin/env python3
+import rospkg
+import actionlib
+
+import os
+
+# python linear algebra library
+import numpy as np
+
+# library for interacting with ROS and TF tree
+import rospy
+import mrover
+
+from mrover.msg import LanderAlignAction
+from mrover.msg import LanderAlignGoal
+
+
+class ClientDummy:
+    def __init__(self):
+        self.landerClient = actionlib.SimpleActionClient('LanderAlignAction', LanderAlignAction)
+        self.landerClient.wait_for_server()
+        print("finished init")
+
+
+    def start_lander_align(self) -> None:
+        print("start")
+        goal = LanderAlignGoal()
+        self.landerClient.send_goal(goal)
+        print("finished start")
+        pass
+
+    def stop_lander_align(self) -> None:
+        print("stop")
+        self.landerClient.cancel_goal()
+        pass
+
+
+
+def main():
+    # initialize the node
+    rospy.init_node("ClientDummy")
+    dummy = ClientDummy()
+
+    dummy.start_lander_align()
+    rospy.sleep(4)
+    dummy.stop_lander_align()
+
+    # let the callback functions run asynchronously in the background
+    rospy.spin()
+
+
+if __name__ == "__main__":
+    main()