Commit 6669ef9d authored by Jakob Dommaschk's avatar Jakob Dommaschk
Browse files

Add landing_controller package

parent 585ccc96
import rclpy
from rclpy.node import Node
import math
from geometry_msgs.msg import Twist, Vector3
from px4_msgs.msg import Timesync, OffboardControlMode, TrajectorySetpoint, VehicleCommand
class VelocityCommander(Node):
def __init__(self):
super().__init__('velocity_commander')
# class variables
self.keep_alive = True
self.key = -1
self.t_sync = 0
self.vel = Vector3()
self.pub_counter = 0
# subscription callbacks
self.sub_timesync = self.create_subscription(
Timesync,
'Timesync_PubSubTopic',
self.process_timesync_callback,
1)
self.sub_vel = self.create_subscription(
Twist,
'cmd_vel',
self.process_vel_callback,
1)
# init publications
self.offb_control_mode_publisher = self.create_publisher(OffboardControlMode, "/OffboardControlMode_PubSubTopic", 10)
self.traj_sp_publisher = self.create_publisher(TrajectorySetpoint, "/TrajectorySetpoint_PubSubTopic", 10)
self.veh_cmd_publisher = self.create_publisher(VehicleCommand, "/VehicleCommand_PubSubTopic", 10)
self.pub_vel_timer = self.create_timer(.1, self.timer_callback)
self.pub_offb_control_mode()
def timer_callback(self):
# change to offboard mode after 10 loops
if self.pub_counter == 10:
self.pub_vehicle_command(VehicleCommand.VEHICLE_CMD_DO_SET_MODE, 1., 6.)
self.arm()
print("Arming")
print(VehicleCommand.VEHICLE_CMD_DO_SET_MODE)
self.pub_offb_control_mode()
self.pub_trajectory_setpoint()
# stop counter
if self.pub_counter < 11:
self.pub_counter = self.pub_counter + 1
def pub_offb_control_mode(self):
mode = OffboardControlMode()
mode.timestamp = self.t_sync
mode.position = True
mode.velocity = True
mode.acceleration = False
mode.attitude = False
mode.body_rate = False
self.offb_control_mode_publisher.publish(mode)
def pub_trajectory_setpoint(self):
# without timesync, the message will not be accepted
if self.t_sync == 0:
return
sp = TrajectorySetpoint()
sp.timestamp = self.t_sync
sp.x = math.nan
sp.y = math.nan
sp.z = -3.
sp.vx = self.vel.x
sp.vy = self.vel.y
sp.vz = self.vel.z
self.traj_sp_publisher.publish(sp)
def process_timesync_callback(self, msg):
self.t_sync = msg.timestamp
def process_vel_callback(self, msg):
self.vel = msg.linear
def arm(self):
self.pub_vehicle_command(VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM, 1., 0.)
def pub_vehicle_command(self, command, param1, param2):
cmd = VehicleCommand()
cmd.timestamp = self.t_sync
cmd.param1 = param1
cmd.param2 = param2
cmd.command = command
cmd.target_system = 1
cmd.target_component = 1
cmd.source_system = 1
cmd.source_component = 1
cmd.from_external = True
self.veh_cmd_publisher.publish(cmd)
def main(args=None):
rclpy.init(args=args)
vel_com = VelocityCommander()
rclpy.spin(vel_com)
# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
vel_com.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>landing_controller</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="dommaschk@campus-tu-berlin.de">jakob</maintainer>
<license>TODO: License declaration</license>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<exec_depend>geometry_msgs</exec_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
[develop]
script-dir=$base/lib/landing_controller
[install]
install-scripts=$base/lib/landing_controller
from setuptools import setup
package_name = 'landing_controller'
setup(
name=package_name,
version='0.0.0',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='jakob',
maintainer_email='dommaschk@campus-tu-berlin.de',
description='TODO: Package description',
license='TODO: License declaration',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'velocity_commander = landing_controller.velocity_commander:main'
],
},
)
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_copyright.main import main
import pytest
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found errors'
# Copyright 2017 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_flake8.main import main_with_errors
import pytest
@pytest.mark.flake8
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
assert rc == 0, \
'Found %d code style errors / warnings:\n' % len(errors) + \
'\n'.join(errors)
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_pep257.main import main
import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found code style errors / warnings'
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