├── .clang-format ├── mav_msgs ├── cmake │ └── export_flags.cmake ├── msg │ ├── GpsWaypoint.msg │ ├── FilteredSensorData.msg │ ├── AttitudeThrust.msg │ ├── TorqueThrust.msg │ ├── Actuators.msg │ ├── RateThrust.msg │ ├── RollPitchYawrateThrust.msg │ └── Status.msg ├── CMakeLists.txt ├── include │ └── mav_msgs │ │ ├── default_values.h │ │ ├── default_topics.h │ │ ├── common.h │ │ ├── eigen_mav_msgs.h │ │ └── conversions.h ├── package.xml └── CHANGELOG.rst ├── mav_planning_msgs ├── msg │ ├── PolynomialTrajectory.msg │ ├── PolynomialTrajectory4D.msg │ ├── Point2D.msg │ ├── PointCloudWithPose.msg │ ├── Polygon2D.msg │ ├── PolygonWithHoles.msg │ ├── PolygonWithHolesStamped.msg │ ├── PolynomialSegment4D.msg │ └── PolynomialSegment.msg ├── srv │ ├── ChangeNameService.srv │ ├── PolygonService.srv │ └── PlannerService.srv ├── mainpage.dox ├── CHANGELOG.rst ├── package.xml ├── CMakeLists.txt └── include │ └── mav_planning_msgs │ ├── eigen_planning_msgs.h │ ├── conversions_deprecated.h │ └── conversions.h ├── mav_state_machine_msgs ├── msg │ └── StartStopTask.msg ├── srv │ └── RunTaskService.srv ├── CMakeLists.txt └── package.xml ├── mav_comm ├── CMakeLists.txt ├── CHANGELOG.rst └── package.xml ├── README.md └── mav_system_msgs ├── msg ├── CpuInfo.msg └── ProcessInfo.msg ├── CMakeLists.txt └── package.xml /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Google 2 | DerivePointerAlignment: true -------------------------------------------------------------------------------- /mav_msgs/cmake/export_flags.cmake: -------------------------------------------------------------------------------- 1 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 2 | 3 | -------------------------------------------------------------------------------- /mav_planning_msgs/msg/PolynomialTrajectory.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | PolynomialSegment[] segments 3 | -------------------------------------------------------------------------------- /mav_state_machine_msgs/msg/StartStopTask.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | string task_name 4 | bool start -------------------------------------------------------------------------------- /mav_planning_msgs/msg/PolynomialTrajectory4D.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | PolynomialSegment4D[] segments 3 | -------------------------------------------------------------------------------- /mav_planning_msgs/msg/Point2D.msg: -------------------------------------------------------------------------------- 1 | # This contains the position of a 2D point. 2 | float64 x 3 | float64 y 4 | -------------------------------------------------------------------------------- /mav_comm/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mav_comm) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /mav_planning_msgs/msg/PointCloudWithPose.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/TransformStamped sensor_pose 3 | sensor_msgs/PointCloud2 cloud_in_sensor_frame -------------------------------------------------------------------------------- /mav_planning_msgs/srv/ChangeNameService.srv: -------------------------------------------------------------------------------- 1 | #request fields 2 | string name 3 | --- 4 | # True on success, false on failure 5 | bool success 6 | string message 7 | -------------------------------------------------------------------------------- /mav_planning_msgs/msg/Polygon2D.msg: -------------------------------------------------------------------------------- 1 | # A specification of a 2D polygon where the first and last points are assumed to be connected. 2 | mav_planning_msgs/Point2D[] points 3 | -------------------------------------------------------------------------------- /mav_planning_msgs/msg/PolygonWithHoles.msg: -------------------------------------------------------------------------------- 1 | # A message to define a 2D polygon with holes. 2 | mav_planning_msgs/Polygon2D hull 3 | mav_planning_msgs/Polygon2D[] holes 4 | -------------------------------------------------------------------------------- /mav_planning_msgs/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b mav_planning_msgs 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /mav_planning_msgs/msg/PolygonWithHolesStamped.msg: -------------------------------------------------------------------------------- 1 | # A message to define a 2D polygon with holes, stamp, and altitude above ground. 2 | Header header 3 | float64 altitude 4 | mav_planning_msgs/PolygonWithHoles polygon 5 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | mav_comm 2 | ======== 3 | 4 | This repository contains message and service definitions used for mavs. All future message definitions go in here, existing ones in other stacks should be moved here where possible. 5 | -------------------------------------------------------------------------------- /mav_state_machine_msgs/srv/RunTaskService.srv: -------------------------------------------------------------------------------- 1 | #request fields 2 | # Name of task 3 | string task_name 4 | # True to start task, False to stop/abort 5 | bool start 6 | --- 7 | # True on success, false on failure to start task 8 | bool success 9 | -------------------------------------------------------------------------------- /mav_planning_msgs/srv/PolygonService.srv: -------------------------------------------------------------------------------- 1 | # A service to set a new polygon with holes. 2 | # Request fields: 3 | mav_planning_msgs/PolygonWithHolesStamped polygon # The new polygon. 4 | --- 5 | # Response fields: 6 | bool success # True on success, false on polygon error. 7 | -------------------------------------------------------------------------------- /mav_msgs/msg/GpsWaypoint.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | float64 latitude # latitude in degree 4 | float64 longitude # longitude in degree 5 | float64 altitude # above start-up point 6 | float64 heading # GPS heading 7 | float64 maxSpeed # maximum approach speed 8 | float64 maxAcc # maximum approach accelerations 9 | -------------------------------------------------------------------------------- /mav_msgs/msg/FilteredSensorData.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | geometry_msgs/Vector3 accelerometer # acceleration in vehicle frame [m/s^2] 4 | geometry_msgs/Vector3 gyroscope # rotational velocity in vehicle frame [rad/s] 5 | geometry_msgs/Vector3 magnetometer # Magnetometer measurements in vehicle frame [uT] 6 | float64 barometer # Height from barometer relative to start-up point [m] 7 | -------------------------------------------------------------------------------- /mav_system_msgs/msg/CpuInfo.msg: -------------------------------------------------------------------------------- 1 | # Miscellaneous information about CPU status. 2 | # Written by Marco Tranzatto (see: https://github.com/ethz-asl/mav_eurathlon/blob/master/mav_eurathlon_msgs/msg/CpuInfo.msg) 3 | 4 | Header header 5 | 6 | float32[] cpu_percent # Current system-wide CPU utilization as a percentage, per CPU. 7 | ProcessInfo[] heaviest_processes # Info about heaviest running processes. -------------------------------------------------------------------------------- /mav_state_machine_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mav_state_machine_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS message_generation std_msgs) 5 | include_directories(include ${catkin_INCLUDE_DIRS}) 6 | 7 | 8 | add_message_files() 9 | add_service_files() 10 | 11 | generate_messages(DEPENDENCIES std_msgs) 12 | 13 | catkin_package(CATKIN_DEPENDS message_runtime) -------------------------------------------------------------------------------- /mav_system_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mav_system_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS message_generation std_msgs) 5 | include_directories(include ${catkin_INCLUDE_DIRS}) 6 | 7 | 8 | add_message_files() 9 | 10 | #add_service_files() 11 | 12 | generate_messages(DEPENDENCIES std_msgs) 13 | 14 | catkin_package(CATKIN_DEPENDS message_runtime std_msgs) -------------------------------------------------------------------------------- /mav_system_msgs/msg/ProcessInfo.msg: -------------------------------------------------------------------------------- 1 | # Miscellaneous information about process status. 2 | # Written by Marco Tranzatto (see: https://github.com/ethz-asl/mav_eurathlon/blob/master/mav_eurathlon_msgs/msg/ProcessInfo.msg) 3 | 4 | uint32 pid # Process PID 5 | string name # Process name 6 | string username # Name of the user that owns the process 7 | float32 cpu_percent # Process CPU utilization as a percentage 8 | -------------------------------------------------------------------------------- /mav_msgs/msg/AttitudeThrust.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | geometry_msgs/Quaternion attitude # Attitude expressed in the header/frame_id frame. 4 | geometry_msgs/Vector3 thrust # Thrust [N] expressed in the body frame. 5 | # For a fixed-wing, usually the x-component is used. 6 | # For a multi-rotor, usually the z-component is used. 7 | # Set all un-used components to 0. 8 | -------------------------------------------------------------------------------- /mav_planning_msgs/msg/PolynomialSegment4D.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | int32 num_coeffs # order of the polynomial + 1, should match size of x[] 3 | duration segment_time # duration of the segment 4 | float64[] x # coefficients for the x-axis, INCREASING order 5 | float64[] y # coefficients for the y-axis, INCREASING order 6 | float64[] z # coefficients for the z-axis, INCREASING order 7 | float64[] yaw # coefficients for the yaw, INCREASING order 8 | -------------------------------------------------------------------------------- /mav_msgs/msg/TorqueThrust.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # We use the coordinate frames with the following convention: 4 | # x: forward 5 | # y: left 6 | # z: up 7 | 8 | geometry_msgs/Vector3 torque # Torque expressed in the body frame [Nm]. 9 | geometry_msgs/Vector3 thrust # Thrust [N] expressed in the body frame. 10 | # For a fixed-wing, usually the x-component is used. 11 | # For a multi-rotor, usually the z-component is used. 12 | # Set all un-used components to 0. 13 | -------------------------------------------------------------------------------- /mav_system_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mav_system_msgs 4 | 0.0.0 5 | 6 | Messages specific to MAV utils scripts. 7 | 8 | 9 | Christian Lanegger 10 | 11 | TBD 12 | 13 | catkin 14 | message_generation 15 | message_runtime 16 | 17 | std_msgs 18 | 19 | 20 | -------------------------------------------------------------------------------- /mav_msgs/msg/Actuators.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # This message defines lowest level commands to be sent to the actuator(s). 4 | 5 | float64[] angles # Angle of the actuator in [rad]. 6 | # E.g. servo angle of a control surface(not angle of the surface!), orientation-angle of a thruster. 7 | float64[] angular_velocities # Angular velocities of the actuator in [rad/s]. 8 | # E.g. "rpm" of rotors, propellers, thrusters 9 | float64[] normalized # Everything that does not fit the above, normalized between [-1 ... 1]. -------------------------------------------------------------------------------- /mav_state_machine_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mav_state_machine_msgs 4 | 0.0.0 5 | 6 | Messages specific to MAV state machine. 7 | 8 | 9 | Christian Lanegger 10 | 11 | TBD 12 | 13 | catkin 14 | message_generation 15 | std_msgs 16 | message_runtime 17 | 18 | 19 | -------------------------------------------------------------------------------- /mav_msgs/msg/RateThrust.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # We use the coordinate frames with the following convention: 4 | # x: forward 5 | # y: left 6 | # z: up 7 | 8 | geometry_msgs/Vector3 angular_rates # Roll-, pitch-, yaw-rate around body axes [rad/s] 9 | geometry_msgs/Vector3 thrust # Thrust [N] expressed in the body frame. 10 | # For a fixed-wing, usually the x-component is used. 11 | # For a multi-rotor, usually the z-component is used. 12 | # Set all un-used components to 0. 13 | -------------------------------------------------------------------------------- /mav_planning_msgs/srv/PlannerService.srv: -------------------------------------------------------------------------------- 1 | #request fields 2 | geometry_msgs/PoseStamped start_pose #start pose for the planner 3 | geometry_msgs/Vector3 start_velocity 4 | geometry_msgs/PoseStamped goal_pose #start pose for the planner 5 | geometry_msgs/Vector3 goal_velocity 6 | geometry_msgs/Vector3 bounding_box 7 | --- 8 | # True on success, false on planning failure 9 | bool success 10 | # Either contains a polynomial trajectory: 11 | mav_planning_msgs/PolynomialTrajectory polynomial_plan 12 | mav_planning_msgs/PolynomialTrajectory4D polynomial_plan_4D 13 | # or a MultiDOFJointTrajectory containing a sampled path (or straight-line 14 | # waypoints, depending on the planner). 15 | # Only one of these should be non-empty. 16 | trajectory_msgs/MultiDOFJointTrajectory sampled_plan 17 | -------------------------------------------------------------------------------- /mav_planning_msgs/msg/PolynomialSegment.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | int32 num_coeffs # order of the polynomial + 1, should match size of x[] 3 | duration segment_time # duration of the segment 4 | float64[] x # coefficients for the x-axis, INCREASING order 5 | float64[] y # coefficients for the y-axis, INCREASING order 6 | float64[] z # coefficients for the z-axis, INCREASING order 7 | float64[] rx # coefficients for the rotation x-vector, INCREASING order 8 | float64[] ry # coefficients for the rotation y-vector, INCREASING order 9 | float64[] rz # coefficients for the rotation z-vector, INCREASING order 10 | ## For backwards compatibility with underactuated (4DOF) commands): 11 | float64[] yaw # coefficients for the yaw, INCREASING order 12 | 13 | -------------------------------------------------------------------------------- /mav_planning_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mav_planning_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 3.3.3 (2019-08-16) 5 | ------------------ 6 | * Add PolynomialSegment/Trajectory messages with 3D rotation vector. 7 | * Update conversions, deprecate old conversions. 8 | * Update planner service to take 4D or full trajectory message 9 | 10 | 3.3.2 (2018-08-22) 11 | ------------------ 12 | * Fix indigo eigen3 compatibility. 13 | 14 | 3.3.1 (2018-08-21) 15 | ------------------ 16 | * Fix Eigen3 warning. Migration from Jade. 17 | * Add std_msgs, eigen and cmake_modules dependencies. 18 | 19 | 3.3.0 (2018-08-17) 20 | ------------------ 21 | * Add 2D polygon messages and services. 22 | * Add ROS and Eigen conversions. 23 | * Add standard services and trajectory messages. 24 | * Add mav_planning_msgs 25 | * Contributors: Helen Oleynikova, Rik Bähnemann 26 | -------------------------------------------------------------------------------- /mav_comm/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mav_comm 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 3.3.3 (2019-08-16) 5 | ------------------ 6 | * Add 6DOF trajectory compatibility. 7 | * See mav_msgs and mav_planning_msgs changelogs for details. 8 | 9 | 3.3.2 (2018-08-22) 10 | ------------------ 11 | * Fix indigo eigen3 compatibility. 12 | 13 | 3.3.1 (2018-08-21) 14 | ------------------ 15 | * Change maintainer. 16 | * Add dependencies, see planning_msgs changelog for details. 17 | 18 | 3.3.0 (2018-08-17) 19 | ------------------ 20 | * More utilities and lower level UAV kinematics, see mav_msgs changelog for details. 21 | * 2D polygon planning msgs, see planings_msgs changelog for details. 22 | 23 | 3.1.0 (2016-12-01) 24 | ------------------ 25 | * More helper functions, see mav_msgs changelog for details. 26 | 27 | 3.0.0 (2015-08-09) 28 | ------------------ 29 | * Changed API for mav_msgs, see mav_msgs changelog for details. 30 | 31 | 2.0.3 (2015-05-22) 32 | ------------------ 33 | -------------------------------------------------------------------------------- /mav_msgs/msg/RollPitchYawrateThrust.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # We use the coordinate frames with the following convention: 4 | # x: forward 5 | # y: left 6 | # z: up 7 | 8 | # rotation convention (z-y'-x''): 9 | # yaw rotates around fixed frame's z axis 10 | # pitch rotates around new y-axis (y') 11 | # roll rotates around new x-axis (x'') 12 | 13 | # This is a convenience-message to support that low-level (microcontroller-based) state 14 | # estimators may not have knowledge about the absolute yaw. 15 | # Roll- and pitch-angle should be specified in the header/frame_id frame 16 | float64 roll # Roll angle [rad] 17 | float64 pitch # Pitch angle [rad] 18 | float64 yaw_rate # Yaw rate around z-axis [rad/s] 19 | 20 | geometry_msgs/Vector3 thrust # Thrust [N] expressed in the body frame. 21 | # For a fixed-wing, usually the x-component is used. 22 | # For a multi-rotor, usually the z-component is used. 23 | # Set all un-used components to 0. 24 | -------------------------------------------------------------------------------- /mav_comm/package.xml: -------------------------------------------------------------------------------- 1 | 2 | mav_comm 3 | 3.3.3 4 | Contains messages and services for MAV communication 5 | 6 | Rik Bähnemann 7 | 8 | Simon Lynen 9 | Markus Achtelik 10 | Pascal Gohl 11 | Sammy Omari 12 | Michael Burri 13 | Fadri Furrer 14 | Helen Oleynikova 15 | Mina Kamel 16 | Karen Bodie 17 | Rik Bähnemann 18 | 19 | ASL 2.0 20 | 21 | https://github.com/ethz-asl/mav_comm 22 | https://github.com/ethz-asl/mav_comm/issues 23 | 24 | 25 | catkin 26 | 27 | 28 | mav_msgs 29 | mav_planning_msgs 30 | mav_state_machine_msgs 31 | mav_system_msgs 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /mav_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mav_msgs) 3 | 4 | find_package(catkin REQUIRED message_generation std_msgs geometry_msgs trajectory_msgs nav_msgs) 5 | include_directories(include ${catkin_INCLUDE_DIRS}) 6 | 7 | find_package(Eigen3 QUIET) 8 | if(NOT EIGEN3_FOUND) 9 | # ROS == Indigo. 10 | find_package(cmake_modules REQUIRED) 11 | find_package(Eigen REQUIRED) 12 | set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 13 | else() 14 | # ROS > Indigo. 15 | set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 16 | endif() 17 | 18 | add_definitions(-std=c++11) 19 | 20 | add_message_files( 21 | FILES 22 | Actuators.msg 23 | AttitudeThrust.msg 24 | RateThrust.msg 25 | RollPitchYawrateThrust.msg 26 | TorqueThrust.msg 27 | Status.msg 28 | FilteredSensorData.msg 29 | GpsWaypoint.msg 30 | ) 31 | 32 | generate_messages(DEPENDENCIES std_msgs geometry_msgs nav_msgs) 33 | 34 | catkin_package( 35 | INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS} 36 | CATKIN_DEPENDS message_runtime std_msgs geometry_msgs trajectory_msgs nav_msgs 37 | CFG_EXTRAS export_flags.cmake 38 | ) 39 | 40 | install( 41 | DIRECTORY include/${PROJECT_NAME}/ 42 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 43 | FILES_MATCHING PATTERN "*.h" 44 | ) 45 | -------------------------------------------------------------------------------- /mav_planning_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mav_planning_msgs 4 | 3.3.3 5 | 6 | Messages specific to MAV planning, especially polynomial planning. 7 | 8 | 9 | Helen Oleynikova 10 | 11 | Simon Lynen 12 | Markus Achtelik 13 | Pascal Gohl 14 | Sammy Omari 15 | Michael Burri 16 | Fadri Furrer 17 | Helen Oleynikova 18 | Karen Bodie 19 | Rik Bähnemann 20 | 21 | ASL 2.0 22 | 23 | https://github.com/ethz-asl/mav_comm 24 | https://github.com/ethz-asl/mav_comm/issues 25 | 26 | catkin 27 | 28 | cmake_modules 29 | eigen 30 | mav_msgs 31 | message_generation 32 | message_runtime 33 | trajectory_msgs 34 | geometry_msgs 35 | sensor_msgs 36 | std_msgs 37 | 38 | -------------------------------------------------------------------------------- /mav_planning_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mav_planning_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS geometry_msgs sensor_msgs std_msgs message_generation mav_msgs trajectory_msgs) 5 | include_directories(include ${catkin_INCLUDE_DIRS}) 6 | 7 | find_package(Eigen3 QUIET) 8 | if(NOT EIGEN3_FOUND) 9 | # ROS == Indigo. 10 | find_package(cmake_modules REQUIRED) 11 | find_package(Eigen REQUIRED) 12 | set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 13 | else() 14 | # ROS > Indigo. 15 | set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 16 | endif() 17 | 18 | add_message_files( 19 | FILES 20 | Point2D.msg 21 | PointCloudWithPose.msg 22 | Polygon2D.msg 23 | PolygonWithHoles.msg 24 | PolygonWithHolesStamped.msg 25 | PolynomialSegment.msg 26 | PolynomialTrajectory.msg 27 | PolynomialSegment4D.msg 28 | PolynomialTrajectory4D.msg 29 | ) 30 | 31 | add_service_files( 32 | FILES 33 | PlannerService.srv 34 | PolygonService.srv 35 | ChangeNameService.srv 36 | ) 37 | 38 | generate_messages( 39 | DEPENDENCIES geometry_msgs sensor_msgs std_msgs mav_msgs trajectory_msgs 40 | ) 41 | 42 | catkin_package( 43 | INCLUDE_DIRS include ${EIGEN3_INCLUDE_DIRS} 44 | CATKIN_DEPENDS message_runtime geometry_msgs sensor_msgs std_msgs mav_msgs trajectory_msgs 45 | ) 46 | -------------------------------------------------------------------------------- /mav_msgs/include/mav_msgs/default_values.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland 3 | * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland 4 | * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland 5 | * Copyright 2015 Helen Oleynikova, ASL, ETH Zurich, Switzerland 6 | * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland 7 | * 8 | * Licensed under the Apache License, Version 2.0 (the "License"); 9 | * you may not use this file except in compliance with the License. 10 | * You may obtain a copy of the License at 11 | * 12 | * http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | * Unless required by applicable law or agreed to in writing, software 15 | * distributed under the License is distributed on an "AS IS" BASIS, 16 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | * See the License for the specific language governing permissions and 18 | * limitations under the License. 19 | */ 20 | 21 | #ifndef MAV_MSGS_DEFAULT_VALUES_H_ 22 | #define MAV_MSGS_DEFAULT_VALUES_H_ 23 | 24 | #include 25 | 26 | namespace mav_msgs { 27 | 28 | const double kZurichLatitude = 0.8267; 29 | const double kZurichHeight = 405.94; 30 | const double kGravity = MagnitudeOfGravity(kZurichHeight, kZurichLatitude); 31 | } 32 | 33 | #endif /* MAV_MSGS_DEFAULT_VALUES_H_ */ 34 | -------------------------------------------------------------------------------- /mav_msgs/msg/Status.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # If values are not known / available, set to -1 or empty string. 4 | string vehicle_name 5 | string vehicle_type # E.g. firefly, pelican ... 6 | float32 battery_voltage # Battery voltage in V. 7 | string rc_command_mode # Command mode set on the 3 position switch on the rc. 8 | bool command_interface_enabled # Reports whether the serial command interface is enabled. 9 | float32 flight_time # Flight time in s. 10 | float32 system_uptime # MAV uptime in s. 11 | float32 cpu_load # MAV CPU load: 0.0 ... 1.0 12 | 13 | string motor_status # Current motor status: running, stopped, starting, stopping. 14 | bool in_air # True if vehicle is actually in air, false otherwise 15 | 16 | string gps_status # GPS status: lock, no_lock 17 | int32 gps_num_satellites # Number of visible satellites 18 | 19 | string RC_COMMAND_ATTITUDE="attitude_thrust" 20 | string RC_COMMAND_ATTITUDE_HEIGHT="attitude_height" 21 | string RC_COMMAND_POSITION="position" 22 | 23 | string MOTOR_STATUS_RUNNING="running" 24 | string MOTOR_STATUS_STOPPED="stopped" 25 | string MOTOR_STATUS_STARTING="starting" 26 | string MOTOR_STATUS_STOPPING="stopping" 27 | 28 | string GPS_STATUS_LOCK="lock" 29 | string GPS_STATUS_NO_LOCK="no_lock" 30 | -------------------------------------------------------------------------------- /mav_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | mav_msgs 3 | 3.3.3 4 | Package containing messages for communicating with rotary wing MAVs 5 | 6 | Rik Bähnemann 7 | 8 | Simon Lynen 9 | Markus Achtelik 10 | Pascal Gohl 11 | Sammy Omari 12 | Michael Burri 13 | Fadri Furrer 14 | Helen Oleynikova 15 | Mina Kamel 16 | Karen Bodie 17 | Rik Bähnemann 18 | 19 | ASL 2.0 20 | 21 | https://github.com/ethz-asl/mav_comm 22 | https://github.com/ethz-asl/mav_comm/issues 23 | 24 | catkin 25 | 26 | cmake_modules 27 | eigen 28 | geometry_msgs 29 | message_generation 30 | nav_msgs 31 | std_msgs 32 | trajectory_msgs 33 | 34 | eigen 35 | geometry_msgs 36 | message_runtime 37 | nav_msgs 38 | std_msgs 39 | trajectory_msgs 40 | 41 | 42 | -------------------------------------------------------------------------------- /mav_planning_msgs/include/mav_planning_msgs/eigen_planning_msgs.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland 3 | * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland 4 | * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef MAV_PLANNING_MSGS_EIGEN_MAV_MSGS_H 20 | #define MAV_PLANNING_MSGS_EIGEN_MAV_MSGS_H 21 | 22 | #include 23 | #include 24 | 25 | namespace mav_planning_msgs { 26 | 27 | struct EigenPolynomialSegment { 28 | EigenPolynomialSegment() : segment_time_ns(0), num_coeffs(0) {}; 29 | 30 | Eigen::VectorXd x; 31 | Eigen::VectorXd y; 32 | Eigen::VectorXd z; 33 | Eigen::VectorXd yaw; 34 | Eigen::VectorXd rx; 35 | Eigen::VectorXd ry; 36 | Eigen::VectorXd rz; 37 | uint64_t segment_time_ns; 38 | int num_coeffs; 39 | }; 40 | 41 | typedef std::vector EigenPolynomialTrajectory; 42 | 43 | } 44 | 45 | #endif // MAV_PLANNING_MSGS_EIGEN_MAV_MSGS_H 46 | -------------------------------------------------------------------------------- /mav_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mav_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 3.3.3 (2019-08-16) 5 | ------------------ 6 | * Add `degrees_of_freedom` to EigenTrajectoryPoint for 6DOF compatibility. 7 | * Add functions to common.h: 8 | * skewMatrixFromVector, vectorFromSkewMatrix, isRotationMatrix, 9 | * matrixFromRotationVector, vectorFromRotationMatrix, omegaFromRotationVector 10 | * omegaDotFromRotationVector 11 | 12 | 3.3.2 (2018-08-22) 13 | ------------------ 14 | * Fix indigo eigen3 compatibility. 15 | 16 | 3.3.1 (2018-08-21) 17 | ------------------ 18 | * Fix Eigen3 warning. Migration from Jade. 19 | * Change maintainer. 20 | 21 | 3.3.0 (2018-08-17) 22 | ------------------ 23 | * Add time conversion utilities. 24 | * Add motor position and force default topics. 25 | * Add conversion from pose message to Eigen trajectory point. 26 | * Add angular accelerations as member of EigenMavState to calculate motor speeds. 27 | * Contributors: Helen Oleynikova, Karen Bodie, Rik Bähnemann 28 | 29 | 3.2.0 (2017-03-02) 30 | ------------------ 31 | * Access covariance in eigen odometry 32 | * External force default topic 33 | * External wind speed default topic 34 | 35 | 3.1.0 (2016-12-01) 36 | ------------------ 37 | * Add getEulerAngles method to EigenOdometry message. 38 | * Improved quaternionFromMsg unit quaternion checking. 39 | * Add EigenMavState to eigen_mav_msgs. 40 | * Add EigenMavStateFromEigenTrajectoryPoint conversion. 41 | * Add `timestamp_ns` to EigenTrajectoryPoint. 42 | * Add default values in a seperate header. 43 | * Add in_air bool to Status.msg. 44 | * Add many helper function to calculate earth gravitational field based on hight and latitude, get euler angles from quaternion and shortest distance between two yaw angles. 45 | * Contributors: Mina Kamel, Helen Oleynikova, Michael Burri 46 | 47 | 3.0.0 (2015-08-09) 48 | ------------------ 49 | * Dropped "Command" from the names of all messages. 50 | * Converted CommandPositionYawTrajectory message to MultiDOFJointTrajectory (with an additional option to use a geometry_msgs/PoseStamped instead) as the two ways to send trajectory/waypoint commands. 51 | * Added conversions between the Eigen and ROS message types. 52 | * Switched to using full orientation instead of just yaw where appropriate. 53 | * Documented reference frame in the Eigen messages where possible. 54 | * Contributors: Helen Oleynikova, Markus Achtelik 55 | 56 | 2.0.3 (2015-05-22) 57 | ------------------ 58 | * added install target for include 59 | Headers can be included outside of this package. 60 | * Contributors: Fadri Furrer 61 | -------------------------------------------------------------------------------- /mav_msgs/include/mav_msgs/default_topics.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland 3 | * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland 4 | * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland 5 | * Copyright 2015 Helen Oleynikova, ASL, ETH Zurich, Switzerland 6 | * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland 7 | * 8 | * Licensed under the Apache License, Version 2.0 (the "License"); 9 | * you may not use this file except in compliance with the License. 10 | * You may obtain a copy of the License at 11 | * 12 | * http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | * Unless required by applicable law or agreed to in writing, software 15 | * distributed under the License is distributed on an "AS IS" BASIS, 16 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | * See the License for the specific language governing permissions and 18 | * limitations under the License. 19 | */ 20 | 21 | #ifndef DEFAULT_TOPICS_H_ 22 | #define DEFAULT_TOPICS_H_ 23 | 24 | namespace mav_msgs { 25 | namespace default_topics { 26 | 27 | static constexpr char IMU[] = "imu"; 28 | static constexpr char MOTOR_MEASUREMENT[] = "motor_speed"; 29 | static constexpr char MOTOR_POSITION_MEASUREMENT[] = "motor_position"; 30 | static constexpr char MOTOR_FORCE_MEASUREMENT[] = "motor_force"; 31 | static constexpr char MAGNETIC_FIELD[] = "magnetic_field"; 32 | static constexpr char GPS[] = "gps"; 33 | static constexpr char RC[] = "rc"; 34 | static constexpr char STATUS[] = "status"; 35 | static constexpr char FILTERED_SENSOR_DATA[] = "filtered_sensor_data"; 36 | static constexpr char AIR_SPEED[] = "air_speed"; 37 | static constexpr char GROUND_SPEED[] = "ground_speed"; 38 | 39 | static constexpr char COMMAND_ACTUATORS[] = "command/motor_speed"; 40 | static constexpr char COMMAND_RATE_THRUST[] = "command/rate_thrust"; 41 | static constexpr char COMMAND_ROLL_PITCH_YAWRATE_THRUST[] = 42 | "command/roll_pitch_yawrate_thrust"; 43 | static constexpr char COMMAND_ATTITUDE_THRUST[] = "command/attitude_thrust"; 44 | static constexpr char COMMAND_TRAJECTORY[] = "command/trajectory"; 45 | static constexpr char COMMAND_POSE[] = "command/pose"; 46 | static constexpr char COMMAND_GPS_WAYPOINT[] = "command/gps_waypoint"; 47 | 48 | static constexpr char POSE[] = "pose"; 49 | static constexpr char POSE_WITH_COVARIANCE[] = "pose_with_covariance"; 50 | static constexpr char TRANSFORM[] = "transform"; 51 | static constexpr char ODOMETRY[] = "odometry"; 52 | static constexpr char POSITION[] = "position"; 53 | 54 | // Simulation-specific topic names. 55 | static constexpr char WRENCH[] = "wrench"; 56 | static constexpr char WIND_SPEED[] = "wind_speed"; 57 | static constexpr char EXTERNAL_FORCE[] = "external_force"; 58 | 59 | static constexpr char GROUND_TRUTH_POSE[] = "ground_truth/pose"; 60 | static constexpr char GROUND_TRUTH_TWIST[] = "ground_truth/twist"; 61 | 62 | // Flight controller specific topic names 63 | static constexpr char FLIGHT_CONTROLLER_ARM[] = "mavros/cmd/arming"; 64 | static constexpr char FLIGHT_CONTROLLER_SET_MODE[] = "mavros/set_mode"; 65 | static constexpr char FLIGHT_CONTROLLER_STATE[] = "mavros/state"; 66 | static constexpr char FLIGHT_CONTROLLER_BATTERY[] = "mavros/battery"; 67 | 68 | } // end namespace default_topics 69 | } // end namespace mav_msgs 70 | 71 | #endif /* DEFAULT_TOPICS_H_ */ 72 | -------------------------------------------------------------------------------- /mav_planning_msgs/include/mav_planning_msgs/conversions_deprecated.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland 3 | * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland 4 | * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef MAV_PLANNING_MSGS_CONVERSIONS_DEPRECATED_H 20 | #define MAV_PLANNING_MSGS_CONVERSIONS_DEPRECATED_H 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include "mav_planning_msgs/PolynomialSegment4D.h" 27 | #include "mav_planning_msgs/PolynomialTrajectory4D.h" 28 | #include "mav_planning_msgs/eigen_planning_msgs.h" 29 | 30 | namespace mav_planning_msgs { 31 | 32 | /// Converts a PolynomialSegment double array to an Eigen::VectorXd. 33 | inline void vectorFromMsgArray(const PolynomialSegment4D::_x_type& array, 34 | Eigen::VectorXd* x); 35 | 36 | /// Converts an Eigen::VectorXd to a PolynomialSegment double array. 37 | inline void msgArrayFromVector(const Eigen::VectorXd& x, 38 | PolynomialSegment4D::_x_type* array); 39 | 40 | /// Converts a PolynomialSegment message to an EigenPolynomialSegment structure. 41 | inline void eigenPolynomialSegmentFromMsg(const PolynomialSegment4D& msg, 42 | EigenPolynomialSegment* segment) { 43 | assert(segment != NULL); 44 | 45 | vectorFromMsgArray(msg.x, &(segment->x)); 46 | vectorFromMsgArray(msg.y, &(segment->y)); 47 | vectorFromMsgArray(msg.z, &(segment->z)); 48 | vectorFromMsgArray(msg.yaw, &(segment->yaw)); 49 | 50 | segment->segment_time_ns = msg.segment_time.toNSec(); 51 | segment->num_coeffs = msg.num_coeffs; 52 | } 53 | 54 | /// Converts a PolynomialTrajectory message to a EigenPolynomialTrajectory 55 | inline void eigenPolynomialTrajectoryFromMsg( 56 | const PolynomialTrajectory4D& msg, 57 | EigenPolynomialTrajectory* eigen_trajectory) { 58 | assert(eigen_trajectory != NULL); 59 | eigen_trajectory->clear(); 60 | eigen_trajectory->reserve(msg.segments.size()); 61 | for (PolynomialTrajectory4D::_segments_type::const_iterator it = 62 | msg.segments.begin(); 63 | it != msg.segments.end(); ++it) { 64 | EigenPolynomialSegment segment; 65 | eigenPolynomialSegmentFromMsg(*it, &segment); 66 | eigen_trajectory->push_back(segment); 67 | } 68 | } 69 | 70 | 71 | /// Converts an EigenPolynomialSegment to a PolynomialSegment message. Does NOT 72 | /// set the header! 73 | inline void polynomialSegmentMsgFromEigen(const EigenPolynomialSegment& segment, 74 | PolynomialSegment4D* msg) { 75 | assert(msg != NULL); 76 | msgArrayFromVector(segment.x, &(msg->x)); 77 | msgArrayFromVector(segment.y, &(msg->y)); 78 | msgArrayFromVector(segment.z, &(msg->z)); 79 | msgArrayFromVector(segment.yaw, &(msg->yaw)); 80 | 81 | msg->segment_time.fromNSec(segment.segment_time_ns); 82 | msg->num_coeffs = segment.num_coeffs; 83 | } 84 | 85 | /// Converts an EigenPolynomialTrajectory to a PolynomialTrajectory message. 86 | /// Does NOT set the header! 87 | inline void polynomialTrajectoryMsgFromEigen( 88 | const EigenPolynomialTrajectory& eigen_trajectory, 89 | PolynomialTrajectory4D* msg) { 90 | assert(msg != NULL); 91 | msg->segments.reserve(eigen_trajectory.size()); 92 | for (EigenPolynomialTrajectory::const_iterator it = eigen_trajectory.begin(); 93 | it != eigen_trajectory.end(); ++it) { 94 | PolynomialSegment4D segment; 95 | polynomialSegmentMsgFromEigen(*it, &segment); 96 | msg->segments.push_back(segment); 97 | } 98 | } 99 | 100 | } // namespace mav_planning_msgs 101 | 102 | #endif // MAV_PLANNING_MSGS_CONVERSIONS_DEPRECATED_H 103 | -------------------------------------------------------------------------------- /mav_planning_msgs/include/mav_planning_msgs/conversions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland 3 | * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland 4 | * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the "License"); 7 | * you may not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * http://www.apache.org/licenses/LICENSE-2.0 11 | 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an "AS IS" BASIS, 14 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | */ 18 | 19 | #ifndef MAV_PLANNING_MSGS_CONVERSIONS_H 20 | #define MAV_PLANNING_MSGS_CONVERSIONS_H 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include "mav_planning_msgs/PolynomialSegment.h" 27 | #include "mav_planning_msgs/PolynomialTrajectory.h" 28 | #include "mav_planning_msgs/eigen_planning_msgs.h" 29 | 30 | // deprecated 31 | #include "mav_planning_msgs/conversions_deprecated.h" 32 | 33 | namespace mav_planning_msgs { 34 | 35 | /// Converts a PolynomialSegment double array to an Eigen::VectorXd. 36 | inline void vectorFromMsgArray(const PolynomialSegment::_x_type& array, 37 | Eigen::VectorXd* x) { 38 | *x = Eigen::Map(&(array[0]), array.size()); 39 | } 40 | 41 | /// Converts an Eigen::VectorXd to a PolynomialSegment double array. 42 | inline void msgArrayFromVector(const Eigen::VectorXd& x, 43 | PolynomialSegment::_x_type* array) { 44 | array->resize(x.size()); 45 | Eigen::Map map = 46 | Eigen::Map(&((*array)[0]), array->size()); 47 | map = x; 48 | } 49 | 50 | /// Converts a PolynomialSegment message to an EigenPolynomialSegment structure. 51 | inline void eigenPolynomialSegmentFromMsg(const PolynomialSegment& msg, 52 | EigenPolynomialSegment* segment) { 53 | assert(segment != NULL); 54 | 55 | vectorFromMsgArray(msg.x, &(segment->x)); 56 | vectorFromMsgArray(msg.y, &(segment->y)); 57 | vectorFromMsgArray(msg.z, &(segment->z)); 58 | vectorFromMsgArray(msg.yaw, &(segment->yaw)); 59 | vectorFromMsgArray(msg.rx, &(segment->rx)); 60 | vectorFromMsgArray(msg.ry, &(segment->ry)); 61 | vectorFromMsgArray(msg.rz, &(segment->rz)); 62 | 63 | segment->segment_time_ns = msg.segment_time.toNSec(); 64 | segment->num_coeffs = msg.num_coeffs; 65 | } 66 | 67 | /// Converts a PolynomialTrajectory message to a EigenPolynomialTrajectory 68 | inline void eigenPolynomialTrajectoryFromMsg( 69 | const PolynomialTrajectory& msg, 70 | EigenPolynomialTrajectory* eigen_trajectory) { 71 | assert(eigen_trajectory != NULL); 72 | eigen_trajectory->clear(); 73 | eigen_trajectory->reserve(msg.segments.size()); 74 | for (PolynomialTrajectory::_segments_type::const_iterator it = 75 | msg.segments.begin(); 76 | it != msg.segments.end(); ++it) { 77 | EigenPolynomialSegment segment; 78 | eigenPolynomialSegmentFromMsg(*it, &segment); 79 | eigen_trajectory->push_back(segment); 80 | } 81 | } 82 | 83 | /// Converts an EigenPolynomialSegment to a PolynomialSegment message. Does NOT 84 | /// set the header! 85 | inline void polynomialSegmentMsgFromEigen(const EigenPolynomialSegment& segment, 86 | PolynomialSegment* msg) { 87 | assert(msg != NULL); 88 | msgArrayFromVector(segment.x, &(msg->x)); 89 | msgArrayFromVector(segment.y, &(msg->y)); 90 | msgArrayFromVector(segment.z, &(msg->z)); 91 | msgArrayFromVector(segment.yaw, &(msg->yaw)); 92 | msgArrayFromVector(segment.rx, &(msg->rx)); 93 | msgArrayFromVector(segment.ry, &(msg->ry)); 94 | msgArrayFromVector(segment.rz, &(msg->rz)); 95 | 96 | msg->segment_time.fromNSec(segment.segment_time_ns); 97 | msg->num_coeffs = segment.num_coeffs; 98 | } 99 | 100 | /// Converts an EigenPolynomialTrajectory to a PolynomialTrajectory message. 101 | /// Does NOT set the header! 102 | inline void polynomialTrajectoryMsgFromEigen( 103 | const EigenPolynomialTrajectory& eigen_trajectory, 104 | PolynomialTrajectory* msg) { 105 | assert(msg != NULL); 106 | msg->segments.reserve(eigen_trajectory.size()); 107 | for (EigenPolynomialTrajectory::const_iterator it = eigen_trajectory.begin(); 108 | it != eigen_trajectory.end(); ++it) { 109 | PolynomialSegment segment; 110 | polynomialSegmentMsgFromEigen(*it, &segment); 111 | msg->segments.push_back(segment); 112 | } 113 | } 114 | 115 | } // namespace mav_planning_msgs 116 | 117 | #endif // MAV_PLANNING_MSGS_CONVERSIONS_H 118 | -------------------------------------------------------------------------------- /mav_msgs/include/mav_msgs/common.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland 3 | * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland 4 | * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland 5 | * Copyright 2015 Helen Oleynikova, ASL, ETH Zurich, Switzerland 6 | * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland 7 | * 8 | * Licensed under the Apache License, Version 2.0 (the "License"); 9 | * you may not use this file except in compliance with the License. 10 | * You may obtain a copy of the License at 11 | * 12 | * http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | * Unless required by applicable law or agreed to in writing, software 15 | * distributed under the License is distributed on an "AS IS" BASIS, 16 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | * See the License for the specific language governing permissions and 18 | * limitations under the License. 19 | */ 20 | 21 | // Common conversion functions between geometry messages, Eigen types, and yaw. 22 | 23 | #ifndef MAV_MSGS_COMMON_H 24 | #define MAV_MSGS_COMMON_H 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | namespace mav_msgs { 33 | 34 | const double kSmallValueCheck = 1.e-6; 35 | const double kNumNanosecondsPerSecond = 1.e9; 36 | 37 | /// Magnitude of Earth's gravitational field at specific height [m] and latitude 38 | /// [rad] (from wikipedia). 39 | inline double MagnitudeOfGravity(const double height, 40 | const double latitude_radians) { 41 | // gravity calculation constants 42 | const double kGravity_0 = 9.780327; 43 | const double kGravity_a = 0.0053024; 44 | const double kGravity_b = 0.0000058; 45 | const double kGravity_c = 3.155 * 1e-7; 46 | 47 | double sin_squared_latitude = std::sin(latitude_radians) * std::sin(latitude_radians); 48 | double sin_squared_twice_latitude = 49 | std::sin(2 * latitude_radians) * std::sin(2 * latitude_radians); 50 | return kGravity_0 * ((1 + kGravity_a * sin_squared_latitude - 51 | kGravity_b * sin_squared_twice_latitude) - 52 | kGravity_c * height); 53 | } 54 | 55 | inline Eigen::Vector3d vector3FromMsg(const geometry_msgs::Vector3& msg) { 56 | return Eigen::Vector3d(msg.x, msg.y, msg.z); 57 | } 58 | 59 | inline Eigen::Vector3d vector3FromPointMsg(const geometry_msgs::Point& msg) { 60 | return Eigen::Vector3d(msg.x, msg.y, msg.z); 61 | } 62 | 63 | inline Eigen::Quaterniond quaternionFromMsg( 64 | const geometry_msgs::Quaternion& msg) { 65 | // Make sure this always returns a valid Quaternion, even if the message was 66 | // uninitialized or only approximately set. 67 | Eigen::Quaterniond quaternion(msg.w, msg.x, msg.y, msg.z); 68 | if (quaternion.norm() < std::numeric_limits::epsilon()) { 69 | quaternion.setIdentity(); 70 | } else { 71 | quaternion.normalize(); 72 | } 73 | return quaternion; 74 | } 75 | 76 | inline void vectorEigenToMsg(const Eigen::Vector3d& eigen, 77 | geometry_msgs::Vector3* msg) { 78 | assert(msg != NULL); 79 | msg->x = eigen.x(); 80 | msg->y = eigen.y(); 81 | msg->z = eigen.z(); 82 | } 83 | 84 | inline void pointEigenToMsg(const Eigen::Vector3d& eigen, 85 | geometry_msgs::Point* msg) { 86 | assert(msg != NULL); 87 | msg->x = eigen.x(); 88 | msg->y = eigen.y(); 89 | msg->z = eigen.z(); 90 | } 91 | 92 | inline void quaternionEigenToMsg(const Eigen::Quaterniond& eigen, 93 | geometry_msgs::Quaternion* msg) { 94 | assert(msg != NULL); 95 | msg->x = eigen.x(); 96 | msg->y = eigen.y(); 97 | msg->z = eigen.z(); 98 | msg->w = eigen.w(); 99 | } 100 | 101 | /** 102 | * \brief Extracts the yaw part from a quaternion, using RPY / euler (z-y'-z'') 103 | * angles. 104 | * RPY rotates about the fixed axes in the order x-y-z, 105 | * which is the same as euler angles in the order z-y'-x''. 106 | */ 107 | inline double yawFromQuaternion(const Eigen::Quaterniond& q) { 108 | return std::atan2(2.0 * (q.w() * q.z() + q.x() * q.y()), 109 | 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z())); 110 | } 111 | 112 | inline Eigen::Quaterniond quaternionFromYaw(double yaw) { 113 | return Eigen::Quaterniond(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); 114 | } 115 | 116 | inline void setQuaternionMsgFromYaw(double yaw, 117 | geometry_msgs::Quaternion* msg) { 118 | assert(msg != NULL); 119 | Eigen::Quaterniond q_yaw = quaternionFromYaw(yaw); 120 | msg->x = q_yaw.x(); 121 | msg->y = q_yaw.y(); 122 | msg->z = q_yaw.z(); 123 | msg->w = q_yaw.w(); 124 | } 125 | 126 | inline void setAngularVelocityMsgFromYawRate(double yaw_rate, 127 | geometry_msgs::Vector3* msg) { 128 | assert(msg != NULL); 129 | msg->x = 0.0; 130 | msg->y = 0.0; 131 | msg->z = yaw_rate; 132 | } 133 | 134 | inline void getEulerAnglesFromQuaternion(const Eigen::Quaternion& q, 135 | Eigen::Vector3d* euler_angles) { 136 | { 137 | assert(euler_angles != NULL); 138 | 139 | *euler_angles << std::atan2(2.0 * (q.w() * q.x() + q.y() * q.z()), 140 | 1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y())), 141 | std::asin(2.0 * (q.w() * q.y() - q.z() * q.x())), 142 | std::atan2(2.0 * (q.w() * q.z() + q.x() * q.y()), 143 | 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z())); 144 | } 145 | } 146 | 147 | inline void skewMatrixFromVector(const Eigen::Vector3d& vec, 148 | Eigen::Matrix3d* vec_skew) { 149 | assert(vec_skew); 150 | *vec_skew << 0.0f, -vec(2), vec(1), vec(2), 0.0f, -vec(0), -vec(1), vec(0), 151 | 0.0f; 152 | } 153 | 154 | inline bool vectorFromSkewMatrix(const Eigen::Matrix3d& vec_skew, 155 | Eigen::Vector3d* vec) { 156 | assert(vec); 157 | if ((vec_skew + vec_skew.transpose()).norm() < kSmallValueCheck){ 158 | *vec << vec_skew(2,1), vec_skew(0,2), vec_skew(1,0); 159 | return true; 160 | } else { 161 | std::cerr << "[mav_msgs] Matrix is not skew-symmetric." << std::endl; 162 | *vec = Eigen::Vector3d::Zero(); 163 | return false; 164 | } 165 | } 166 | 167 | inline bool isRotationMatrix(const Eigen::Matrix3d& mat){ 168 | // Check that R^T * R = I 169 | if ((mat.transpose() * mat - Eigen::Matrix3d::Identity()).norm() > kSmallValueCheck){ 170 | std::cerr << "[mav_msgs::common] Rotation matrix requirement violated (R^T * R = I)" << std::endl; 171 | return false; 172 | } 173 | // Check that det(R) = 1 174 | if (mat.determinant() - 1.0 > kSmallValueCheck){ 175 | std::cerr << "[mav_msgs::common] Rotation matrix requirement violated (det(R) = 1)" << std::endl; 176 | return false; 177 | } 178 | return true; 179 | } 180 | 181 | // Rotation matrix from rotation vector as described in 182 | // "Computationally Efficient Trajectory Generation for Fully Actuated Multirotor Vehicles" 183 | // Brescianini 2018 184 | inline void matrixFromRotationVector(const Eigen::Vector3d& vec, 185 | Eigen::Matrix3d* mat) { 186 | // R = (I + sin||r|| / ||r||) [r] + ((1 - cos||r||)/||r||^2) [r]^2 187 | // where [r] is the skew matrix of r vector 188 | assert(mat); 189 | double r_norm = vec.norm(); 190 | Eigen::Matrix3d vec_skew_norm = Eigen::Matrix3d::Zero(); 191 | if (r_norm > 0.0){ 192 | skewMatrixFromVector(vec / r_norm, &vec_skew_norm); 193 | } 194 | 195 | *mat = Eigen::Matrix3d::Identity() + vec_skew_norm * std::sin(r_norm) + 196 | vec_skew_norm * vec_skew_norm * (1 - std::cos(r_norm)); 197 | } 198 | 199 | // Rotation vector from rotation matrix as described in 200 | // "Computationally Efficient Trajectory Generation for Fully Actuated Multirotor Vehicles" 201 | // Brescianini 2018 202 | inline bool vectorFromRotationMatrix(const Eigen::Matrix3d& mat, 203 | Eigen::Vector3d* vec) { 204 | // [r] = phi / 2sin(phi) * (R - R^T) 205 | // where [r] is the skew matrix of r vector 206 | // and phi satisfies 1 + 2cos(phi) = trace(R) 207 | assert(vec); 208 | 209 | if (!isRotationMatrix(mat)){ 210 | std::cerr << "[mav_msgs::common] Not a rotation matrix." << std::endl; 211 | return false; 212 | } 213 | 214 | if ((mat - Eigen::Matrix3d::Identity()).norm() < kSmallValueCheck){ 215 | *vec = Eigen::Vector3d::Zero(); 216 | return true; 217 | } 218 | 219 | // Compute cosine of angle and clamp in range [-1, 1] 220 | double cos_phi = (mat.trace() - 1.0) / 2.0; 221 | double cos_phi_clamped = boost::algorithm::clamp(cos_phi, -1.0, 1.0); 222 | double phi = std::acos(cos_phi_clamped); 223 | 224 | if (phi < kSmallValueCheck){ 225 | *vec = Eigen::Vector3d::Zero(); 226 | } else{ 227 | Eigen::Matrix3d vec_skew = (mat - mat.transpose()) * phi / (2.0 * std::sin(phi)); 228 | Eigen::Vector3d vec_unskewed; 229 | if (vectorFromSkewMatrix(vec_skew, &vec_unskewed)){ 230 | *vec = vec_unskewed; 231 | }else{ 232 | return false; 233 | } 234 | 235 | } 236 | return true; 237 | } 238 | 239 | // Calculates angular velocity (omega) from rotation vector derivative 240 | // based on formula derived in "Finite rotations and angular velocity" by Asher 241 | // Peres 242 | inline Eigen::Vector3d omegaFromRotationVector( 243 | const Eigen::Vector3d& rot_vec, const Eigen::Vector3d& rot_vec_vel) 244 | { 245 | double phi = rot_vec.norm(); 246 | if (std::abs(phi) < 1.0e-3) { 247 | // This captures the case of zero rotation 248 | return rot_vec_vel; 249 | } 250 | 251 | double phi_inv = 1.0 / phi; 252 | double phi_2_inv = phi_inv / phi; 253 | double phi_3_inv = phi_2_inv / phi; 254 | 255 | // Create skew-symmetric matrix from rotation vector 256 | Eigen::Matrix3d phi_skew; 257 | skewMatrixFromVector(rot_vec, &phi_skew); 258 | 259 | // Set up matrix to calculate omega 260 | Eigen::Matrix3d W; 261 | W = Eigen::MatrixXd::Identity(3, 3) + phi_skew * (1 - std::cos(phi)) * phi_2_inv + 262 | phi_skew * phi_skew * (phi - std::sin(phi)) * phi_3_inv; 263 | return W * rot_vec_vel; 264 | } 265 | 266 | // Calculates angular acceleration (omegaDot) from rotation vector derivative 267 | // based on formula derived in "Finite rotations and angular velocity" by Asher 268 | // Peres 269 | inline Eigen::Vector3d omegaDotFromRotationVector( 270 | const Eigen::Vector3d& rot_vec, const Eigen::Vector3d& rot_vec_vel, 271 | const Eigen::Vector3d& rot_vec_acc) 272 | { 273 | double phi = rot_vec.norm(); 274 | if (std::abs(phi) < 1.0e-3) { 275 | // This captures the case of zero rotation 276 | return rot_vec_acc; 277 | } 278 | 279 | double phi_dot = rot_vec.dot(rot_vec_vel) / phi; 280 | 281 | double phi_inv = 1.0 / phi; 282 | double phi_2_inv = phi_inv / phi; 283 | double phi_3_inv = phi_2_inv / phi; 284 | double phi_4_inv = phi_3_inv / phi; 285 | 286 | 287 | // Create skew-symmetric matrix from rotation vector and velocity 288 | Eigen::Matrix3d phi_skew; 289 | Eigen::Matrix3d phi_dot_skew; 290 | 291 | skewMatrixFromVector(rot_vec, &phi_skew); 292 | skewMatrixFromVector(rot_vec_vel, &phi_dot_skew); 293 | 294 | // Set up matrices to calculate omega dot 295 | Eigen::Matrix3d W_vel; 296 | Eigen::Matrix3d W_acc; 297 | W_vel = phi_skew * (phi * std::sin(phi) - 2.0f + 2.0f * std::cos(phi)) * phi_dot * 298 | phi_3_inv + 299 | phi_skew * phi_skew * 300 | (-2.0f * phi - phi * std::cos(phi) + 3.0f * std::sin(phi)) * phi_dot * 301 | phi_4_inv + 302 | phi_dot_skew * phi_skew * (phi - std::sin(phi)) * phi_3_inv; 303 | 304 | W_acc = Eigen::MatrixXd::Identity(3, 3) + 305 | phi_skew * (1.0f - std::cos(phi)) * phi_2_inv + 306 | phi_skew * phi_skew * (phi - std::sin(phi)) * phi_3_inv; 307 | 308 | return W_vel * rot_vec_vel + W_acc * rot_vec_acc; 309 | } 310 | 311 | // Calculate the nominal rotor rates given the MAV mass, allocation matrix, 312 | // angular velocity, angular acceleration, and body acceleration (normalized 313 | // thrust). 314 | // 315 | // [torques, thrust]' = A * n^2, where 316 | // torques = J * ang_acc + ang_vel x J 317 | // thrust = m * norm(acc) 318 | // 319 | // The allocation matrix A has of a hexacopter is: 320 | // A = K * B, where 321 | // K = diag(l*c_T, l*c_T, c_M, c_T), 322 | // [ s 1 s -s -1 -s] 323 | // B = [-c 0 c c 0 -c] 324 | // [-1 1 -1 1 -1 1] 325 | // [ 1 1 1 1 1 1], 326 | // l: arm length 327 | // c_T: thrust constant 328 | // c_M: moment constant 329 | // s: sin(30°) 330 | // c: cos(30°) 331 | // 332 | // The inverse can be computed computationally efficient: 333 | // A^-1 \approx B^pseudo * K^-1 334 | inline void getSquaredRotorSpeedsFromAllocationAndState( 335 | const Eigen::MatrixXd& allocation_inv, const Eigen::Vector3d& inertia, 336 | double mass, const Eigen::Vector3d& angular_velocity_B, 337 | const Eigen::Vector3d& angular_acceleration_B, 338 | const Eigen::Vector3d& acceleration_B, 339 | Eigen::VectorXd* rotor_rates_squared) { 340 | const Eigen::Vector3d torque = 341 | inertia.asDiagonal() * angular_acceleration_B + 342 | angular_velocity_B.cross(inertia.asDiagonal() * angular_velocity_B); 343 | const double thrust_force = mass * acceleration_B.norm(); 344 | Eigen::Vector4d input; 345 | input << torque, thrust_force; 346 | *rotor_rates_squared = allocation_inv * input; 347 | } 348 | 349 | inline double nanosecondsToSeconds(int64_t nanoseconds) { 350 | double seconds = nanoseconds / kNumNanosecondsPerSecond; 351 | return seconds; 352 | } 353 | 354 | inline int64_t secondsToNanoseconds(double seconds) { 355 | int64_t nanoseconds = 356 | static_cast(seconds * kNumNanosecondsPerSecond); 357 | return nanoseconds; 358 | } 359 | 360 | } // namespace mav_msgs 361 | 362 | #endif // MAV_MSGS_COMMON_H 363 | -------------------------------------------------------------------------------- /mav_msgs/include/mav_msgs/eigen_mav_msgs.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland 3 | * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland 4 | * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland 5 | * Copyright 2015 Helen Oleynikova, ASL, ETH Zurich, Switzerland 6 | * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland 7 | * 8 | * Licensed under the Apache License, Version 2.0 (the "License"); 9 | * you may not use this file except in compliance with the License. 10 | * You may obtain a copy of the License at 11 | * 12 | * http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | * Unless required by applicable law or agreed to in writing, software 15 | * distributed under the License is distributed on an "AS IS" BASIS, 16 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | * See the License for the specific language governing permissions and 18 | * limitations under the License. 19 | */ 20 | 21 | #ifndef MAV_MSGS_EIGEN_MAV_MSGS_H 22 | #define MAV_MSGS_EIGEN_MAV_MSGS_H 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include "mav_msgs/common.h" 29 | 30 | namespace mav_msgs { 31 | 32 | /// Actuated degrees of freedom. 33 | enum MavActuation { DOF4 = 4, DOF6 = 6 }; 34 | 35 | struct EigenAttitudeThrust { 36 | EigenAttitudeThrust() 37 | : attitude(Eigen::Quaterniond::Identity()), 38 | thrust(Eigen::Vector3d::Zero()) {} 39 | EigenAttitudeThrust(const Eigen::Quaterniond& _attitude, 40 | const Eigen::Vector3d& _thrust) { 41 | attitude = _attitude; 42 | thrust = _thrust; 43 | } 44 | 45 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 46 | Eigen::Quaterniond attitude; 47 | Eigen::Vector3d thrust; 48 | }; 49 | 50 | struct EigenActuators { 51 | // TODO(ffurrer): Find a proper way of initializing :) 52 | 53 | EigenActuators(const Eigen::VectorXd& _angular_velocities) { 54 | angular_velocities = _angular_velocities; 55 | } 56 | 57 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 58 | Eigen::VectorXd angles; // In rad. 59 | Eigen::VectorXd angular_velocities; // In rad/s. 60 | Eigen::VectorXd normalized; // Everything else, normalized [-1 to 1]. 61 | }; 62 | 63 | struct EigenRateThrust { 64 | EigenRateThrust() 65 | : angular_rates(Eigen::Vector3d::Zero()), 66 | thrust(Eigen::Vector3d::Zero()) {} 67 | 68 | EigenRateThrust(const Eigen::Vector3d& _angular_rates, 69 | const Eigen::Vector3d _thrust) 70 | : angular_rates(_angular_rates), thrust(_thrust) {} 71 | 72 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 73 | Eigen::Vector3d angular_rates; 74 | Eigen::Vector3d thrust; 75 | }; 76 | 77 | struct EigenTorqueThrust { 78 | EigenTorqueThrust() 79 | : torque(Eigen::Vector3d::Zero()), thrust(Eigen::Vector3d::Zero()) {} 80 | 81 | EigenTorqueThrust(const Eigen::Vector3d& _torque, 82 | const Eigen::Vector3d _thrust) 83 | : torque(_torque), thrust(_thrust) {} 84 | 85 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 86 | Eigen::Vector3d torque; 87 | Eigen::Vector3d thrust; 88 | }; 89 | 90 | struct EigenRollPitchYawrateThrust { 91 | EigenRollPitchYawrateThrust() 92 | : roll(0.0), pitch(0.0), yaw_rate(0.0), thrust(Eigen::Vector3d::Zero()) {} 93 | 94 | EigenRollPitchYawrateThrust(double _roll, double _pitch, double _yaw_rate, 95 | const Eigen::Vector3d& _thrust) 96 | : roll(_roll), pitch(_pitch), yaw_rate(_yaw_rate), thrust(_thrust) {} 97 | 98 | double roll; 99 | double pitch; 100 | double yaw_rate; 101 | Eigen::Vector3d thrust; 102 | }; 103 | 104 | /** 105 | * \brief Container holding the state of a MAV: position, velocity, attitude and 106 | * angular velocity. 107 | * In addition, holds the acceleration expressed in body coordinates, 108 | * which is what the accelerometer 109 | * usually measures. 110 | */ 111 | class EigenMavState { 112 | public: 113 | typedef std::vector> Vector; 115 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 116 | 117 | /// Initializes all members to zero / identity. 118 | EigenMavState() 119 | : position_W(Eigen::Vector3d::Zero()), 120 | velocity_W(Eigen::Vector3d::Zero()), 121 | acceleration_B(Eigen::Vector3d::Zero()), 122 | orientation_W_B(Eigen::Quaterniond::Identity()), 123 | angular_velocity_B(Eigen::Vector3d::Zero()), 124 | angular_acceleration_B(Eigen::Vector3d::Zero()) {} 125 | 126 | EigenMavState(const Eigen::Vector3d& position_W, 127 | const Eigen::Vector3d& velocity_W, 128 | const Eigen::Vector3d& acceleration_B, 129 | const Eigen::Quaterniond& orientation_W_B, 130 | const Eigen::Vector3d& angular_velocity_B, 131 | const Eigen::Vector3d& angular_acceleration_B) 132 | : position_W(position_W), 133 | velocity_W(velocity_W), 134 | acceleration_B(acceleration_B), 135 | orientation_W_B(orientation_W_B), 136 | angular_velocity_B(angular_velocity_B), 137 | angular_acceleration_B(angular_acceleration_B) {} 138 | 139 | std::string toString() const { 140 | std::stringstream ss; 141 | ss << "position: " << position_W.transpose() << std::endl 142 | << "velocity: " << velocity_W.transpose() << std::endl 143 | << "acceleration_body: " << acceleration_B.transpose() << std::endl 144 | << "orientation (w-x-y-z): " << orientation_W_B.w() << " " 145 | << orientation_W_B.x() << " " << orientation_W_B.y() << " " 146 | << orientation_W_B.z() << " " << std::endl 147 | << "angular_velocity_body: " << angular_velocity_B.transpose() 148 | << std::endl 149 | << "angular_acceleration_body: " << angular_acceleration_B.transpose() 150 | << std::endl; 151 | 152 | return ss.str(); 153 | } 154 | 155 | Eigen::Vector3d position_W; 156 | Eigen::Vector3d velocity_W; 157 | Eigen::Vector3d acceleration_B; 158 | Eigen::Quaterniond orientation_W_B; 159 | Eigen::Vector3d angular_velocity_B; 160 | Eigen::Vector3d angular_acceleration_B; 161 | }; 162 | 163 | struct EigenTrajectoryPoint { 164 | typedef std::vector> Vector; 166 | EigenTrajectoryPoint() 167 | : timestamp_ns(-1), 168 | time_from_start_ns(0), 169 | position_W(Eigen::Vector3d::Zero()), 170 | velocity_W(Eigen::Vector3d::Zero()), 171 | acceleration_W(Eigen::Vector3d::Zero()), 172 | jerk_W(Eigen::Vector3d::Zero()), 173 | snap_W(Eigen::Vector3d::Zero()), 174 | orientation_W_B(Eigen::Quaterniond::Identity()), 175 | angular_velocity_W(Eigen::Vector3d::Zero()), 176 | angular_acceleration_W(Eigen::Vector3d::Zero()), 177 | degrees_of_freedom(MavActuation::DOF4) {} 178 | 179 | EigenTrajectoryPoint(int64_t _time_from_start_ns, 180 | const Eigen::Vector3d& _position, 181 | const Eigen::Vector3d& _velocity, 182 | const Eigen::Vector3d& _acceleration, 183 | const Eigen::Vector3d& _jerk, 184 | const Eigen::Vector3d& _snap, 185 | const Eigen::Quaterniond& _orientation, 186 | const Eigen::Vector3d& _angular_velocity, 187 | const Eigen::Vector3d& _angular_acceleration, 188 | const MavActuation& _degrees_of_freedom = MavActuation::DOF4) 189 | : time_from_start_ns(_time_from_start_ns), 190 | position_W(_position), 191 | velocity_W(_velocity), 192 | acceleration_W(_acceleration), 193 | jerk_W(_jerk), 194 | snap_W(_snap), 195 | orientation_W_B(_orientation), 196 | angular_velocity_W(_angular_velocity), 197 | angular_acceleration_W(_angular_acceleration), 198 | degrees_of_freedom(_degrees_of_freedom) {} 199 | 200 | EigenTrajectoryPoint(int64_t _time_from_start_ns, 201 | const Eigen::Vector3d& _position, 202 | const Eigen::Vector3d& _velocity, 203 | const Eigen::Vector3d& _acceleration, 204 | const Eigen::Vector3d& _jerk, 205 | const Eigen::Vector3d& _snap, 206 | const Eigen::Quaterniond& _orientation, 207 | const Eigen::Vector3d& _angular_velocity, 208 | const MavActuation& _degrees_of_freedom = MavActuation::DOF4) 209 | : EigenTrajectoryPoint(_time_from_start_ns, _position, _velocity, 210 | _acceleration, _jerk, _snap, _orientation, 211 | _angular_velocity, Eigen::Vector3d::Zero(), _degrees_of_freedom) {} 212 | 213 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 214 | int64_t timestamp_ns; // Time since epoch, negative value = invalid timestamp. 215 | int64_t time_from_start_ns; 216 | Eigen::Vector3d position_W; 217 | Eigen::Vector3d velocity_W; 218 | Eigen::Vector3d acceleration_W; 219 | Eigen::Vector3d jerk_W; 220 | Eigen::Vector3d snap_W; 221 | 222 | Eigen::Quaterniond orientation_W_B; 223 | Eigen::Vector3d angular_velocity_W; 224 | Eigen::Vector3d angular_acceleration_W; 225 | MavActuation degrees_of_freedom; 226 | 227 | // Accessors for making dealing with orientation/angular velocity easier. 228 | inline double getYaw() const { return yawFromQuaternion(orientation_W_B); } 229 | inline double getYawRate() const { return angular_velocity_W.z(); } 230 | inline double getYawAcc() const { return angular_acceleration_W.z(); } 231 | // WARNING: sets roll and pitch to 0. 232 | inline void setFromYaw(double yaw) { 233 | orientation_W_B = quaternionFromYaw(yaw); 234 | } 235 | inline void setFromYawRate(double yaw_rate) { 236 | angular_velocity_W.x() = 0.0; 237 | angular_velocity_W.y() = 0.0; 238 | angular_velocity_W.z() = yaw_rate; 239 | } 240 | inline void setFromYawAcc(double yaw_acc) { 241 | angular_acceleration_W.x() = 0.0; 242 | angular_acceleration_W.y() = 0.0; 243 | angular_acceleration_W.z() = yaw_acc; 244 | } 245 | 246 | std::string toString() const { 247 | std::stringstream ss; 248 | ss << "position: " << position_W.transpose() << std::endl 249 | << "velocity: " << velocity_W.transpose() << std::endl 250 | << "acceleration: " << acceleration_W.transpose() << std::endl 251 | << "jerk: " << jerk_W.transpose() << std::endl 252 | << "snap: " << snap_W.transpose() << std::endl 253 | << "yaw: " << getYaw() << std::endl 254 | << "yaw_rate: " << getYawRate() << std::endl 255 | << "yaw_acc: " << getYawAcc() << std::endl; 256 | 257 | return ss.str(); 258 | } 259 | }; 260 | 261 | // Operator overload to transform Trajectory Points according to the Eigen 262 | // interfaces (uses operator* for this). 263 | // Has to be outside of class. 264 | // Example: 265 | // Eigen::Affine3d transform; EigenTrajectoryPoint point; 266 | // EigenTrajectoryPoint transformed = transform * point; 267 | inline EigenTrajectoryPoint operator*(const Eigen::Affine3d& lhs, 268 | const EigenTrajectoryPoint& rhs) { 269 | EigenTrajectoryPoint transformed(rhs); 270 | transformed.position_W = lhs * rhs.position_W; 271 | transformed.velocity_W = lhs.rotation() * rhs.velocity_W; 272 | transformed.acceleration_W = lhs.rotation() * rhs.acceleration_W; 273 | transformed.jerk_W = lhs.rotation() * rhs.jerk_W; 274 | transformed.snap_W = lhs.rotation() * rhs.snap_W; 275 | transformed.orientation_W_B = lhs.rotation() * rhs.orientation_W_B; 276 | transformed.angular_velocity_W = lhs.rotation() * rhs.angular_velocity_W; 277 | transformed.angular_acceleration_W = 278 | lhs.rotation() * rhs.angular_acceleration_W; 279 | return transformed; 280 | } 281 | 282 | struct EigenOdometry { 283 | EigenOdometry() 284 | : timestamp_ns(-1), 285 | position_W(Eigen::Vector3d::Zero()), 286 | orientation_W_B(Eigen::Quaterniond::Identity()), 287 | velocity_B(Eigen::Vector3d::Zero()), 288 | angular_velocity_B(Eigen::Vector3d::Zero()) {} 289 | 290 | EigenOdometry(const Eigen::Vector3d& _position, 291 | const Eigen::Quaterniond& _orientation, 292 | const Eigen::Vector3d& _velocity_body, 293 | const Eigen::Vector3d& _angular_velocity) 294 | : position_W(_position), 295 | orientation_W_B(_orientation), 296 | velocity_B(_velocity_body), 297 | angular_velocity_B(_angular_velocity) {} 298 | 299 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 300 | int64_t timestamp_ns; // Time since epoch, negative value = invalid timestamp. 301 | Eigen::Vector3d position_W; 302 | Eigen::Quaterniond orientation_W_B; 303 | Eigen::Vector3d velocity_B; // Velocity in expressed in the Body frame! 304 | Eigen::Vector3d angular_velocity_B; 305 | Eigen::Matrix pose_covariance_; 306 | Eigen::Matrix twist_covariance_; 307 | 308 | // Accessors for making dealing with orientation/angular velocity easier. 309 | inline double getYaw() const { return yawFromQuaternion(orientation_W_B); } 310 | inline void getEulerAngles(Eigen::Vector3d* euler_angles) const { 311 | getEulerAnglesFromQuaternion(orientation_W_B, euler_angles); 312 | } 313 | inline double getYawRate() const { return angular_velocity_B.z(); } 314 | // WARNING: sets roll and pitch to 0. 315 | inline void setFromYaw(double yaw) { 316 | orientation_W_B = quaternionFromYaw(yaw); 317 | } 318 | inline void setFromYawRate(double yaw_rate) { 319 | angular_velocity_B.x() = 0.0; 320 | angular_velocity_B.y() = 0.0; 321 | angular_velocity_B.z() = yaw_rate; 322 | } 323 | 324 | inline Eigen::Vector3d getVelocityWorld() const { 325 | return orientation_W_B * velocity_B; 326 | } 327 | inline void setVelocityWorld(const Eigen::Vector3d& velocity_world) { 328 | velocity_B = orientation_W_B.inverse() * velocity_world; 329 | } 330 | }; 331 | 332 | // TODO(helenol): replaced with aligned allocator headers from Simon. 333 | #define MAV_MSGS_CONCATENATE(x, y) x##y 334 | #define MAV_MSGS_CONCATENATE2(x, y) MAV_MSGS_CONCATENATE(x, y) 335 | #define MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EIGEN_TYPE) \ 336 | typedef std::vector> \ 337 | MAV_MSGS_CONCATENATE2(EIGEN_TYPE, Vector); \ 338 | typedef std::deque> \ 339 | MAV_MSGS_CONCATENATE2(EIGEN_TYPE, Deque); 340 | 341 | MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenAttitudeThrust) 342 | MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenActuators) 343 | MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenRateThrust) 344 | MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenTrajectoryPoint) 345 | MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenRollPitchYawrateThrust) 346 | MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenOdometry) 347 | } 348 | 349 | #endif // MAV_MSGS_EIGEN_MAV_MSGS_H 350 | -------------------------------------------------------------------------------- /mav_msgs/include/mav_msgs/conversions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland 3 | * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland 4 | * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland 5 | * Copyright 2015 Helen Oleynikova, ASL, ETH Zurich, Switzerland 6 | * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland 7 | * 8 | * Licensed under the Apache License, Version 2.0 (the "License"); 9 | * you may not use this file except in compliance with the License. 10 | * You may obtain a copy of the License at 11 | * 12 | * http://www.apache.org/licenses/LICENSE-2.0 13 | 14 | * Unless required by applicable law or agreed to in writing, software 15 | * distributed under the License is distributed on an "AS IS" BASIS, 16 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | * See the License for the specific language governing permissions and 18 | * limitations under the License. 19 | */ 20 | 21 | // Conversion functions between Eigen types and MAV ROS message types. 22 | 23 | #ifndef MAV_MSGS_CONVERSIONS_H 24 | #define MAV_MSGS_CONVERSIONS_H 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "mav_msgs/Actuators.h" 36 | #include "mav_msgs/AttitudeThrust.h" 37 | #include "mav_msgs/RateThrust.h" 38 | #include "mav_msgs/RollPitchYawrateThrust.h" 39 | #include "mav_msgs/TorqueThrust.h" 40 | #include "mav_msgs/common.h" 41 | #include "mav_msgs/default_values.h" 42 | #include "mav_msgs/eigen_mav_msgs.h" 43 | 44 | namespace mav_msgs { 45 | 46 | inline void eigenAttitudeThrustFromMsg(const AttitudeThrust& msg, 47 | EigenAttitudeThrust* attitude_thrust) { 48 | assert(attitude_thrust != NULL); 49 | 50 | attitude_thrust->attitude = quaternionFromMsg(msg.attitude); 51 | attitude_thrust->thrust = vector3FromMsg(msg.thrust); 52 | } 53 | 54 | inline void eigenActuatorsFromMsg(const Actuators& msg, 55 | EigenActuators* actuators) { 56 | assert(actuators != NULL); 57 | 58 | // Angle of the actuator in [rad]. 59 | actuators->angles.resize(msg.angles.size()); 60 | for (unsigned int i = 0; i < msg.angles.size(); ++i) { 61 | actuators->angles[i] = msg.angles[i]; 62 | } 63 | 64 | // Angular velocities of the actuator in [rad/s]. 65 | actuators->angular_velocities.resize(msg.angular_velocities.size()); 66 | for (unsigned int i = 0; i < msg.angular_velocities.size(); ++i) { 67 | actuators->angular_velocities[i] = msg.angular_velocities[i]; 68 | } 69 | 70 | // Normalized: Everything that does not fit the above, normalized 71 | // between [-1 ... 1]. 72 | actuators->normalized.resize(msg.normalized.size()); 73 | for (unsigned int i = 0; i < msg.normalized.size(); ++i) { 74 | actuators->normalized[i] = msg.normalized[i]; 75 | } 76 | } 77 | 78 | inline void eigenRateThrustFromMsg(const RateThrust& msg, 79 | EigenRateThrust* rate_thrust) { 80 | assert(rate_thrust != NULL); 81 | 82 | rate_thrust->angular_rates = vector3FromMsg(msg.angular_rates); 83 | rate_thrust->thrust = vector3FromMsg(msg.thrust); 84 | } 85 | 86 | inline void eigenTorqueThrustFromMsg(const TorqueThrust& msg, 87 | EigenTorqueThrust* torque_thrust) { 88 | assert(torque_thrust != NULL); 89 | 90 | torque_thrust->torque = vector3FromMsg(msg.torque); 91 | torque_thrust->thrust = vector3FromMsg(msg.thrust); 92 | } 93 | 94 | inline void eigenRollPitchYawrateThrustFromMsg( 95 | const RollPitchYawrateThrust& msg, 96 | EigenRollPitchYawrateThrust* roll_pitch_yawrate_thrust) { 97 | assert(roll_pitch_yawrate_thrust != NULL); 98 | 99 | roll_pitch_yawrate_thrust->roll = msg.roll; 100 | roll_pitch_yawrate_thrust->pitch = msg.pitch; 101 | roll_pitch_yawrate_thrust->yaw_rate = msg.yaw_rate; 102 | roll_pitch_yawrate_thrust->thrust = vector3FromMsg(msg.thrust); 103 | } 104 | 105 | inline void eigenOdometryFromMsg(const nav_msgs::Odometry& msg, 106 | EigenOdometry* odometry) { 107 | assert(odometry != NULL); 108 | odometry->timestamp_ns = msg.header.stamp.toNSec(); 109 | odometry->position_W = mav_msgs::vector3FromPointMsg(msg.pose.pose.position); 110 | odometry->orientation_W_B = 111 | mav_msgs::quaternionFromMsg(msg.pose.pose.orientation); 112 | odometry->velocity_B = mav_msgs::vector3FromMsg(msg.twist.twist.linear); 113 | odometry->angular_velocity_B = 114 | mav_msgs::vector3FromMsg(msg.twist.twist.angular); 115 | odometry->pose_covariance_ = 116 | Eigen::Map>(msg.pose.covariance.data()); 117 | odometry->twist_covariance_ = Eigen::Map>( 118 | msg.twist.covariance.data()); 119 | } 120 | 121 | inline void eigenTrajectoryPointFromTransformMsg( 122 | const geometry_msgs::TransformStamped& msg, 123 | EigenTrajectoryPoint* trajectory_point) { 124 | assert(trajectory_point != NULL); 125 | 126 | ros::Time timestamp = msg.header.stamp; 127 | 128 | trajectory_point->timestamp_ns = timestamp.toNSec(); 129 | trajectory_point->position_W = vector3FromMsg(msg.transform.translation); 130 | trajectory_point->orientation_W_B = quaternionFromMsg(msg.transform.rotation); 131 | trajectory_point->velocity_W.setZero(); 132 | trajectory_point->angular_velocity_W.setZero(); 133 | trajectory_point->acceleration_W.setZero(); 134 | trajectory_point->angular_acceleration_W.setZero(); 135 | trajectory_point->jerk_W.setZero(); 136 | trajectory_point->snap_W.setZero(); 137 | } 138 | 139 | // 2 versions of this: one for PoseStamped (which fills in the timestamps 140 | // correctly and should be used if at all possible), and one for a raw pose 141 | // message. 142 | inline void eigenTrajectoryPointFromPoseMsg( 143 | const geometry_msgs::Pose& msg, EigenTrajectoryPoint* trajectory_point) { 144 | assert(trajectory_point != NULL); 145 | 146 | trajectory_point->position_W = vector3FromPointMsg(msg.position); 147 | trajectory_point->orientation_W_B = quaternionFromMsg(msg.orientation); 148 | trajectory_point->velocity_W.setZero(); 149 | trajectory_point->angular_velocity_W.setZero(); 150 | trajectory_point->acceleration_W.setZero(); 151 | trajectory_point->angular_acceleration_W.setZero(); 152 | trajectory_point->jerk_W.setZero(); 153 | trajectory_point->snap_W.setZero(); 154 | } 155 | 156 | inline void eigenTrajectoryPointFromPoseMsg( 157 | const geometry_msgs::PoseStamped& msg, 158 | EigenTrajectoryPoint* trajectory_point) { 159 | assert(trajectory_point != NULL); 160 | 161 | ros::Time timestamp = msg.header.stamp; 162 | 163 | trajectory_point->timestamp_ns = timestamp.toNSec(); 164 | eigenTrajectoryPointFromPoseMsg(msg.pose, trajectory_point); 165 | } 166 | 167 | /** 168 | * \brief Computes the MAV state (position, velocity, attitude, angular 169 | * velocity, angular acceleration) from the flat state. 170 | * Additionally, computes the acceleration expressed in body coordinates, 171 | * i.e. the acceleration measured by the IMU. No air-drag is assumed here. 172 | * 173 | * \param[in] acceleration Acceleration of the MAV, expressed in world 174 | * coordinates. 175 | * \param[in] jerk Jerk of the MAV, expressed in world coordinates. 176 | * \param[in] snap Snap of the MAV, expressed in world coordinates. 177 | * \param[in] yaw Yaw angle of the MAV, expressed in world coordinates. 178 | * \param[in] yaw_rate Yaw rate, expressed in world coordinates. 179 | * \param[in] yaw_acceleration Yaw acceleration, expressed in world coordinates. 180 | * \param[in] magnitude_of_gravity Magnitude of the gravity vector. 181 | * \param[out] orientation Quaternion representing the attitude of the MAV. 182 | * \param[out] acceleration_body Acceleration expressed in body coordinates, 183 | * i.e. the acceleration usually by the IMU. 184 | * \param[out] angular_velocity_body Angular velocity of the MAV, expressed in 185 | * body coordinates. 186 | * \param[out] angular_acceleration_body Angular acceleration of the MAV, 187 | * expressed in body coordinates. 188 | */ 189 | void EigenMavStateFromEigenTrajectoryPoint( 190 | const Eigen::Vector3d& acceleration, const Eigen::Vector3d& jerk, 191 | const Eigen::Vector3d& snap, double yaw, double yaw_rate, 192 | double yaw_acceleration, double magnitude_of_gravity, 193 | Eigen::Quaterniond* orientation, Eigen::Vector3d* acceleration_body, 194 | Eigen::Vector3d* angular_velocity_body, 195 | Eigen::Vector3d* angular_acceleration_body); 196 | 197 | /// Convenience function with default value for the magnitude of the gravity 198 | /// vector. 199 | inline void EigenMavStateFromEigenTrajectoryPoint( 200 | const Eigen::Vector3d& acceleration, const Eigen::Vector3d& jerk, 201 | const Eigen::Vector3d& snap, double yaw, double yaw_rate, 202 | double yaw_acceleration, Eigen::Quaterniond* orientation, 203 | Eigen::Vector3d* acceleration_body, Eigen::Vector3d* angular_velocity_body, 204 | Eigen::Vector3d* angular_acceleration_body) { 205 | EigenMavStateFromEigenTrajectoryPoint( 206 | acceleration, jerk, snap, yaw, yaw_rate, yaw_acceleration, kGravity, 207 | orientation, acceleration_body, angular_velocity_body, 208 | angular_acceleration_body); 209 | } 210 | 211 | /// Convenience function with EigenTrajectoryPoint as input and EigenMavState as 212 | /// output. 213 | inline void EigenMavStateFromEigenTrajectoryPoint( 214 | const EigenTrajectoryPoint& trajectory_point, double magnitude_of_gravity, 215 | EigenMavState* mav_state) { 216 | assert(mav_state != NULL); 217 | if (trajectory_point.degrees_of_freedom == MavActuation::DOF4) { 218 | EigenMavStateFromEigenTrajectoryPoint( 219 | trajectory_point.acceleration_W, trajectory_point.jerk_W, 220 | trajectory_point.snap_W, trajectory_point.getYaw(), 221 | trajectory_point.getYawRate(), trajectory_point.getYawAcc(), 222 | magnitude_of_gravity, &(mav_state->orientation_W_B), 223 | &(mav_state->acceleration_B), &(mav_state->angular_velocity_B), 224 | &(mav_state->angular_acceleration_B)); 225 | mav_state->position_W = trajectory_point.position_W; 226 | mav_state->velocity_W = trajectory_point.velocity_W; 227 | } else { 228 | // Assume fully actuated vehicle, can track trajectory point. 229 | Eigen::Matrix3d R = trajectory_point.orientation_W_B.toRotationMatrix(); 230 | mav_state->position_W = trajectory_point.position_W; 231 | mav_state->velocity_W = trajectory_point.velocity_W; 232 | mav_state->acceleration_B = 233 | R.transpose() * (trajectory_point.acceleration_W + 234 | Eigen::Vector3d(0.0, 0.0, magnitude_of_gravity)); 235 | mav_state->orientation_W_B = trajectory_point.orientation_W_B; 236 | mav_state->angular_velocity_B = 237 | R.transpose() * trajectory_point.angular_velocity_W; 238 | mav_state->angular_acceleration_B = 239 | R.transpose() * trajectory_point.angular_acceleration_W; 240 | } 241 | } 242 | 243 | /** 244 | * \brief Convenience function with EigenTrajectoryPoint as input and 245 | * EigenMavState as output 246 | * with default value for the magnitude of the gravity vector. 247 | */ 248 | inline void EigenMavStateFromEigenTrajectoryPoint( 249 | const EigenTrajectoryPoint& flat_state, EigenMavState* mav_state) { 250 | EigenMavStateFromEigenTrajectoryPoint(flat_state, kGravity, mav_state); 251 | } 252 | 253 | inline void EigenMavStateFromEigenTrajectoryPoint( 254 | const Eigen::Vector3d& acceleration, const Eigen::Vector3d& jerk, 255 | const Eigen::Vector3d& snap, double yaw, double yaw_rate, 256 | double yaw_acceleration, double magnitude_of_gravity, 257 | Eigen::Quaterniond* orientation, Eigen::Vector3d* acceleration_body, 258 | Eigen::Vector3d* angular_velocity_body, 259 | Eigen::Vector3d* angular_acceleration_body) { 260 | // Mapping from flat state to full state following to Mellinger [1]: 261 | // See [1]: Mellinger, Daniel Warren. "Trajectory generation and control for 262 | // quadrotors." (2012), Phd-thesis, p. 15ff. 263 | // http://repository.upenn.edu/cgi/viewcontent.cgi?article=1705&context=edissertations 264 | // 265 | // zb = acc+[0 0 magnitude_of_gravity]'; 266 | // thrust = norm(zb); 267 | // zb = zb / thrust; 268 | // 269 | // xc = [cos(yaw) sin(yaw) 0]'; 270 | // 271 | // yb = cross(zb, xc); 272 | // yb = yb/norm(yb); 273 | // 274 | // xb = cross(yb, zb); 275 | // 276 | // q(:,i) = rot2quat([xb yb zb]); 277 | // 278 | // h_w = 1/thrust*(acc_dot - zb' * acc_dot * zb; 279 | // 280 | // w(1,i) = -h_w'*yb; 281 | // w(2,i) = h_w'*xb; 282 | // w(3,i) = yaw_dot*[0 0 1]*zb; 283 | 284 | assert(orientation != nullptr); 285 | assert(acceleration_body != nullptr); 286 | assert(angular_velocity_body != nullptr); 287 | assert(angular_acceleration_body != nullptr); 288 | 289 | Eigen::Vector3d xb; 290 | Eigen::Vector3d yb; 291 | Eigen::Vector3d zb(acceleration); 292 | 293 | zb[2] += magnitude_of_gravity; 294 | const double thrust = zb.norm(); 295 | const double inv_thrust = 1.0 / thrust; 296 | zb = zb * inv_thrust; 297 | 298 | yb = zb.cross(Eigen::Vector3d(cos(yaw), sin(yaw), 0.0)); 299 | yb.normalize(); 300 | 301 | xb = yb.cross(zb); 302 | 303 | const Eigen::Matrix3d R((Eigen::Matrix3d() << xb, yb, zb).finished()); 304 | 305 | const Eigen::Vector3d h_w = 306 | inv_thrust * (jerk - double(zb.transpose() * jerk) * zb); 307 | 308 | *orientation = Eigen::Quaterniond(R); 309 | *acceleration_body = R.transpose() * zb * thrust; 310 | (*angular_velocity_body)[0] = -h_w.transpose() * yb; 311 | (*angular_velocity_body)[1] = h_w.transpose() * xb; 312 | (*angular_velocity_body)[2] = yaw_rate * zb[2]; 313 | 314 | // Calculate angular accelerations. 315 | const Eigen::Vector3d wcrossz = (*angular_velocity_body).cross(zb); 316 | const Eigen::Vector3d wcrosswcrossz = (*angular_velocity_body).cross(wcrossz); 317 | const Eigen::Vector3d h_a = 318 | inv_thrust * (snap - double(zb.transpose() * snap) * zb) - 2 * wcrossz - 319 | wcrosswcrossz + double(zb.transpose() * wcrosswcrossz) * zb; 320 | 321 | (*angular_acceleration_body)[0] = -h_a.transpose() * yb; 322 | (*angular_acceleration_body)[1] = h_a.transpose() * xb; 323 | (*angular_acceleration_body)[2] = yaw_acceleration * zb[2]; 324 | } 325 | 326 | inline void eigenTrajectoryPointFromMsg( 327 | const trajectory_msgs::MultiDOFJointTrajectoryPoint& msg, 328 | EigenTrajectoryPoint* trajectory_point) { 329 | assert(trajectory_point != NULL); 330 | 331 | if (msg.transforms.empty()) { 332 | ROS_ERROR("MultiDofJointTrajectoryPoint is empty."); 333 | return; 334 | } 335 | 336 | if (msg.transforms.size() > 1) { 337 | ROS_WARN( 338 | "MultiDofJointTrajectoryPoint message should have one joint, but has " 339 | "%lu. Using first joint.", 340 | msg.transforms.size()); 341 | } 342 | 343 | trajectory_point->time_from_start_ns = msg.time_from_start.toNSec(); 344 | trajectory_point->position_W = vector3FromMsg(msg.transforms[0].translation); 345 | trajectory_point->orientation_W_B = 346 | quaternionFromMsg(msg.transforms[0].rotation); 347 | if (msg.velocities.size() > 0) { 348 | trajectory_point->velocity_W = vector3FromMsg(msg.velocities[0].linear); 349 | trajectory_point->angular_velocity_W = 350 | vector3FromMsg(msg.velocities[0].angular); 351 | } else { 352 | trajectory_point->velocity_W.setZero(); 353 | trajectory_point->angular_velocity_W.setZero(); 354 | } 355 | if (msg.accelerations.size() > 0) { 356 | trajectory_point->acceleration_W = 357 | vector3FromMsg(msg.accelerations[0].linear); 358 | trajectory_point->angular_acceleration_W = 359 | vector3FromMsg(msg.accelerations[0].angular); 360 | } else { 361 | trajectory_point->acceleration_W.setZero(); 362 | trajectory_point->angular_acceleration_W.setZero(); 363 | } 364 | trajectory_point->jerk_W.setZero(); 365 | trajectory_point->snap_W.setZero(); 366 | } 367 | 368 | inline void eigenTrajectoryPointVectorFromMsg( 369 | const trajectory_msgs::MultiDOFJointTrajectory& msg, 370 | EigenTrajectoryPointVector* trajectory) { 371 | assert(trajectory != NULL); 372 | trajectory->clear(); 373 | for (const auto& msg_point : msg.points) { 374 | EigenTrajectoryPoint point; 375 | eigenTrajectoryPointFromMsg(msg_point, &point); 376 | trajectory->push_back(point); 377 | } 378 | } 379 | 380 | inline void eigenTrajectoryPointDequeFromMsg( 381 | const trajectory_msgs::MultiDOFJointTrajectory& msg, 382 | EigenTrajectoryPointDeque* trajectory) { 383 | assert(trajectory != NULL); 384 | trajectory->clear(); 385 | for (const auto& msg_point : msg.points) { 386 | EigenTrajectoryPoint point; 387 | eigenTrajectoryPointFromMsg(msg_point, &point); 388 | trajectory->push_back(point); 389 | } 390 | } 391 | 392 | // In all these conversions, client is responsible for filling in message 393 | // header. 394 | inline void msgActuatorsFromEigen(const EigenActuators& actuators, 395 | Actuators* msg) { 396 | assert(msg != NULL); 397 | 398 | msg->angles.resize(actuators.angles.size()); 399 | for (unsigned int i = 0; i < actuators.angles.size(); ++i) { 400 | msg->angles[i] = actuators.angles[i]; 401 | } 402 | 403 | msg->angular_velocities.resize(actuators.angular_velocities.size()); 404 | for (unsigned int i = 0; i < actuators.angular_velocities.size(); ++i) { 405 | msg->angular_velocities[i] = actuators.angular_velocities[i]; 406 | } 407 | 408 | msg->normalized.resize(actuators.normalized.size()); 409 | for (unsigned int i = 0; i < actuators.normalized.size(); ++i) { 410 | msg->normalized[i] = actuators.normalized[i]; 411 | } 412 | } 413 | 414 | inline void msgAttitudeThrustFromEigen( 415 | const EigenAttitudeThrust& attitude_thrust, AttitudeThrust* msg) { 416 | assert(msg != NULL); 417 | quaternionEigenToMsg(attitude_thrust.attitude, &msg->attitude); 418 | vectorEigenToMsg(attitude_thrust.thrust, &msg->thrust); 419 | } 420 | 421 | inline void msgRateThrustFromEigen(EigenRateThrust& rate_thrust, 422 | RateThrust* msg) { 423 | assert(msg != NULL); 424 | vectorEigenToMsg(rate_thrust.angular_rates, &msg->angular_rates); 425 | vectorEigenToMsg(rate_thrust.thrust, &msg->thrust); 426 | } 427 | 428 | inline void msgTorqueThrustFromEigen(EigenTorqueThrust& torque_thrust, 429 | TorqueThrust* msg) { 430 | assert(msg != NULL); 431 | vectorEigenToMsg(torque_thrust.torque, &msg->torque); 432 | vectorEigenToMsg(torque_thrust.thrust, &msg->thrust); 433 | } 434 | 435 | inline void msgRollPitchYawrateThrustFromEigen( 436 | const EigenRollPitchYawrateThrust& roll_pitch_yawrate_thrust, 437 | RollPitchYawrateThrust* msg) { 438 | assert(msg != NULL); 439 | msg->roll = roll_pitch_yawrate_thrust.roll; 440 | msg->pitch = roll_pitch_yawrate_thrust.pitch; 441 | msg->yaw_rate = roll_pitch_yawrate_thrust.yaw_rate; 442 | vectorEigenToMsg(roll_pitch_yawrate_thrust.thrust, &msg->thrust); 443 | } 444 | 445 | inline void msgOdometryFromEigen(const EigenOdometry& odometry, 446 | nav_msgs::Odometry* msg) { 447 | assert(msg != NULL); 448 | 449 | if (odometry.timestamp_ns >= 0) { 450 | msg->header.stamp.fromNSec(odometry.timestamp_ns); 451 | } 452 | pointEigenToMsg(odometry.position_W, &msg->pose.pose.position); 453 | quaternionEigenToMsg(odometry.orientation_W_B, &msg->pose.pose.orientation); 454 | 455 | vectorEigenToMsg(odometry.velocity_B, &msg->twist.twist.linear); 456 | vectorEigenToMsg(odometry.angular_velocity_B, &msg->twist.twist.angular); 457 | } 458 | 459 | // WARNING: discards all derivatives, etc. 460 | inline void msgPoseStampedFromEigenTrajectoryPoint( 461 | const EigenTrajectoryPoint& trajectory_point, 462 | geometry_msgs::PoseStamped* msg) { 463 | if (trajectory_point.timestamp_ns >= 0) { 464 | msg->header.stamp.fromNSec(trajectory_point.timestamp_ns); 465 | } 466 | pointEigenToMsg(trajectory_point.position_W, &msg->pose.position); 467 | quaternionEigenToMsg(trajectory_point.orientation_W_B, 468 | &msg->pose.orientation); 469 | } 470 | 471 | inline void msgMultiDofJointTrajectoryPointFromEigen( 472 | const EigenTrajectoryPoint& trajectory_point, 473 | trajectory_msgs::MultiDOFJointTrajectoryPoint* msg) { 474 | assert(msg != NULL); 475 | 476 | msg->time_from_start.fromNSec(trajectory_point.time_from_start_ns); 477 | msg->transforms.resize(1); 478 | msg->velocities.resize(1); 479 | msg->accelerations.resize(1); 480 | 481 | vectorEigenToMsg(trajectory_point.position_W, 482 | &msg->transforms[0].translation); 483 | quaternionEigenToMsg(trajectory_point.orientation_W_B, 484 | &msg->transforms[0].rotation); 485 | vectorEigenToMsg(trajectory_point.velocity_W, &msg->velocities[0].linear); 486 | vectorEigenToMsg(trajectory_point.angular_velocity_W, 487 | &msg->velocities[0].angular); 488 | vectorEigenToMsg(trajectory_point.acceleration_W, 489 | &msg->accelerations[0].linear); 490 | vectorEigenToMsg(trajectory_point.angular_acceleration_W, 491 | &msg->accelerations[0].angular); 492 | } 493 | 494 | inline void msgMultiDofJointTrajectoryFromEigen( 495 | const EigenTrajectoryPoint& trajectory_point, const std::string& link_name, 496 | trajectory_msgs::MultiDOFJointTrajectory* msg) { 497 | assert(msg != NULL); 498 | trajectory_msgs::MultiDOFJointTrajectoryPoint point_msg; 499 | msgMultiDofJointTrajectoryPointFromEigen(trajectory_point, &point_msg); 500 | 501 | msg->joint_names.clear(); 502 | msg->points.clear(); 503 | msg->joint_names.push_back(link_name); 504 | msg->points.push_back(point_msg); 505 | } 506 | 507 | inline void msgMultiDofJointTrajectoryFromEigen( 508 | const EigenTrajectoryPoint& trajectory_point, 509 | trajectory_msgs::MultiDOFJointTrajectory* msg) { 510 | msgMultiDofJointTrajectoryFromEigen(trajectory_point, "base_link", msg); 511 | } 512 | 513 | // Convenience method to quickly create a trajectory from a single waypoint. 514 | inline void msgMultiDofJointTrajectoryFromPositionYaw( 515 | const Eigen::Vector3d& position, double yaw, 516 | trajectory_msgs::MultiDOFJointTrajectory* msg) { 517 | assert(msg != NULL); 518 | 519 | EigenTrajectoryPoint point; 520 | point.position_W = position; 521 | point.setFromYaw(yaw); 522 | 523 | msgMultiDofJointTrajectoryFromEigen(point, msg); 524 | } 525 | 526 | inline void msgMultiDofJointTrajectoryFromEigen( 527 | const EigenTrajectoryPointVector& trajectory, const std::string& link_name, 528 | trajectory_msgs::MultiDOFJointTrajectory* msg) { 529 | assert(msg != NULL); 530 | 531 | if (trajectory.empty()) { 532 | ROS_ERROR("EigenTrajectoryPointVector is empty."); 533 | return; 534 | } 535 | 536 | msg->joint_names.clear(); 537 | msg->joint_names.push_back(link_name); 538 | msg->points.clear(); 539 | 540 | for (const auto& trajectory_point : trajectory) { 541 | trajectory_msgs::MultiDOFJointTrajectoryPoint point_msg; 542 | msgMultiDofJointTrajectoryPointFromEigen(trajectory_point, &point_msg); 543 | msg->points.push_back(point_msg); 544 | } 545 | } 546 | 547 | inline void msgMultiDofJointTrajectoryFromEigen( 548 | const EigenTrajectoryPointVector& trajectory, 549 | trajectory_msgs::MultiDOFJointTrajectory* msg) { 550 | msgMultiDofJointTrajectoryFromEigen(trajectory, "base_link", msg); 551 | } 552 | 553 | inline void msgMultiDofJointTrajectoryFromEigen( 554 | const EigenTrajectoryPointDeque& trajectory, const std::string& link_name, 555 | trajectory_msgs::MultiDOFJointTrajectory* msg) { 556 | assert(msg != NULL); 557 | 558 | if (trajectory.empty()) { 559 | ROS_ERROR("EigenTrajectoryPointVector is empty."); 560 | return; 561 | } 562 | 563 | msg->joint_names.clear(); 564 | msg->joint_names.push_back(link_name); 565 | msg->points.clear(); 566 | 567 | for (const auto& trajectory_point : trajectory) { 568 | trajectory_msgs::MultiDOFJointTrajectoryPoint point_msg; 569 | msgMultiDofJointTrajectoryPointFromEigen(trajectory_point, &point_msg); 570 | msg->points.push_back(point_msg); 571 | } 572 | } 573 | 574 | inline void msgMultiDofJointTrajectoryFromEigen( 575 | const EigenTrajectoryPointDeque& trajectory, 576 | trajectory_msgs::MultiDOFJointTrajectory* msg) { 577 | msgMultiDofJointTrajectoryFromEigen(trajectory, "base_link", msg); 578 | } 579 | 580 | } // end namespace mav_msgs 581 | 582 | #endif // MAV_MSGS_CONVERSIONS_H 583 | --------------------------------------------------------------------------------