Commit b88642eb authored by Jakob Dommaschk's avatar Jakob Dommaschk
Browse files

cleanup

parent b29ac331
......@@ -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
......
......@@ -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()):
......
......@@ -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='',
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment