cleanup

src/landing_controller/landing_controller/velocity_commander.py

@@ -9,7 +9,7 @@ from px4_msgs.msg import Timesync, OffboardControlMode, TrajectorySetpoint, Vehi
 
 class VelocityCommander(Node):
 
-    # parameters
+    # constants
     TOPICS = {
         "CMD_VEL":    'cmd_vel',
         "PX4_TIME":   'Timesync_PubSubTopic',
@@ -48,12 +48,6 @@ class VelocityCommander(Node):
         self.pub_vel_timer = self.create_timer(.1, self.timer_callback)
 
     def timer_callback(self):
-        # change to offboard mode after 10 loops
-        if self.pub_counter == 10:
-            # set offboard mode
-            self.pub_vehicle_command(VehicleCommand.VEHICLE_CMD_DO_SET_MODE, 1., 6.)
-            #self.arm()
-
         self.pub_offb_control_mode()
         self.pub_trajectory_setpoint()
 
@@ -87,7 +81,7 @@ class VelocityCommander(Node):
 
         sp.x = math.nan
         sp.y = math.nan
-        sp.z = -3.
+        sp.z = -1.5
 
         sp.yaw = math.nan # should not yaw right at startup
 

src/landing_controller/landing_controller/velocity_controller.py

@@ -5,8 +5,6 @@ from rclpy.node import Node
 
 from time import sleep
 
-import math
-
 from std_msgs.msg import Header, Float32
 from geometry_msgs.msg import TransformStamped, Twist, Vector3, Transform, Quaternion
 
@@ -22,28 +20,25 @@ from landing_controller.vector_math import *
 
 class VelocityController(Node):
 
-    # parameters
+    # namings
     TOPICS = {
         "P_GAIN" : "/landing_controller/p_gain"
     }
     TOPICS["CMD_VEL"] = VelocityCommander.TOPICS["CMD_VEL"]
 
-    FRAMES = {
-        "ARUCO"   : "aruco_marker"
-    }
-
-    # time in seconds in which the timeout function is called
-    # timeout is earlist executed after ARUCO_TIMEOUT seconds
-    # and latest after 2*ARUCO_TIMEOUT seconds
-    ARUCO_TIMEOUT = .5   
-
-
     def __init__(self):
         super().__init__('velocity_controller')
 
+        # parameters
+        # static transformation from pixhawk to camera frame
+        TF_PIX_TO_CAMERA = Transform(
+            Vector3(z=.1),      # Translation from Pixhawk to Camera
+            Quaternion()        # Rotation from Pixhawk to Camera
+        )
+        self.gains = {"P": 1.} # Initial controller gains
+
         # class variables
         self.aruco_timeout_reset = True
-        self.gains = {"P": 1.} # Initial controller gains
 
         # set up transform listener
         self.tf_buf = Buffer()
@@ -56,8 +51,7 @@ class VelocityController(Node):
 
         # pixhawk to camera -> dependent on drone construction
         self.init_static_frame('pixhawk', 'camera', 
-                Vector3(z=.1),      # Translation from Pixhawk to Camera
-                Quaternion())       # Rotation from Pixhawk to Camera
+                self.TF_PIX_TO_CAMERA.Translation, self.TF_PIX_TO_CAMERA.Rotation) 
 
         # set up transform broadcaster (for debug)
         self.tf_broadcaster = TransformBroadcaster(self)
@@ -126,33 +120,6 @@ class VelocityController(Node):
             print(u)
 
             self.pub_xy_vel(u)
-        
-
-    def aruco_timer_callback(self):
-        """Checks aruco_timeout_variable and executes timeout if it was not set
-        since the last call
-        """
-
-        """
-        targets = ['ned','pixhawk','camera','aruco']
-        for target in targets:
-            if self.tf_buf.can_transform('map',target, Time()):
-                print(target)"""
-
-        
-        if self.tf_buf.can_transform('ned','camera', Time()):
-            self.tf_ned_aruco = self.tf_buf.lookup_transform('ned','camera', Time())
-            print(transform_apply(Vector3(y=1.), self.tf_ned_aruco.transform))
-
-        else:
-            print("At least one frame does not exist")
-            
-
-        """
-        if self.aruco_timeout_reset == True:
-            self.timeout()
-
-        self.aruco_timeout_reset = True"""
 
 
     def timeout(self):
@@ -178,7 +145,7 @@ class VelocityController(Node):
 
 
     def publish_debug_frames(self):
-        frames = ['ned', 'camera', 'pixhawk']
+        frames = ['ned', 'camera', 'pixhawk', 'aruco_marker']
 
         for frame in frames:
             if self.tf_buf.can_transform('map',frame, Time()):

src/landing_controller/launch/general.launch.py

@@ -19,6 +19,13 @@ def generate_launch_description():
 
 
     return launch.LaunchDescription([
+        launch_ros.actions.Node(
+            package='landing_controller',
+            namespace='',
+            executable='velocity_controller',
+            output='screen',
+            name=['velocity_controller']),
+
         launch_ros.actions.Node(
             package='landing_controller',
             namespace='',