/usr/lib/python3/dist-packages/morse/middleware/ros/read_asctec_ctrl_input.py is in python3-morse-simulator 1.4-2.
This file is owned by root:root, with mode 0o644.
The actual contents of the file can be viewed below.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 | import logging; logger = logging.getLogger("morse." + __name__)
import math
from morse.core import blenderapi
try:
from asctec_msgs.msg import CtrlInput
except ImportError:
if not blenderapi.fake:
raise ImportError("To use the ROS CtrlInput interface for drones, you"
" must manually install the ROS 'asctec_msgs' package.")
from morse.middleware.ros import ROSSubscriber
class CtrlInputReader(ROSSubscriber):
""" Subscribe to a CtrlInput topic and set pitch,roll,yaw,thrust local data. """
ros_class = CtrlInput
def update(self, message):
max_angle = math.radians(30)
max_yaw_rate = math.radians(90)
yaw_deadband = 5
asctec_scale = 2047
roll = message.roll / asctec_scale * max_angle
pitch = message.pitch / asctec_scale * max_angle
if math.fabs(message.yaw) > yaw_deadband:
yaw_rate = max_yaw_rate / asctec_scale * (message.yaw - math.copysign(yaw_deadband, message.yaw))
else:
yaw_rate = 0.0
thrust = message.thrust / 4095
self.data["pitch"] = pitch
self.data["roll"] = roll
self.data["yaw"] = yaw_rate
self.data["thrust"] = thrust
logger.debug("new RPY thrust setpoint: (% .2f % .2f % .3f %3f)",
math.degrees(roll), math.degrees(pitch), math.degrees(yaw_rate), message.thrust)
|