diff options
Diffstat (limited to 'src/platforms/ros')
-rwxr-xr-x | src/platforms/ros/eigen_math.h | 19 | ||||
-rw-r--r-- | src/platforms/ros/geo.cpp | 173 | ||||
-rw-r--r-- | src/platforms/ros/nodes/README.md | 22 | ||||
-rw-r--r-- | src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp | 149 | ||||
-rw-r--r-- | src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h | 62 | ||||
-rw-r--r-- | src/platforms/ros/nodes/commander/commander.cpp | 116 | ||||
-rw-r--r-- | src/platforms/ros/nodes/commander/commander.h | 69 | ||||
-rw-r--r-- | src/platforms/ros/nodes/manual_input/manual_input.cpp | 118 | ||||
-rw-r--r-- | src/platforms/ros/nodes/manual_input/manual_input.h | 74 | ||||
-rw-r--r-- | src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 276 | ||||
-rwxr-xr-x | src/platforms/ros/perf_counter.cpp | 176 | ||||
-rw-r--r-- | src/platforms/ros/px4_nodehandle.cpp | 44 | ||||
-rw-r--r-- | src/platforms/ros/px4_publisher.cpp | 41 | ||||
-rw-r--r-- | src/platforms/ros/px4_ros_impl.cpp | 56 |
14 files changed, 1395 insertions, 0 deletions
diff --git a/src/platforms/ros/eigen_math.h b/src/platforms/ros/eigen_math.h new file mode 100755 index 000000000..c7271c157 --- /dev/null +++ b/src/platforms/ros/eigen_math.h @@ -0,0 +1,19 @@ +/* + * eigen_math.h + * + * Created on: Aug 25, 2014 + * Author: roman + */ + +#ifndef EIGEN_MATH_H_ +#define EIGEN_MATH_H_ + + +struct eigen_matrix_instance { + int numRows; + int numCols; + float *pData; +}; + + +#endif /* EIGEN_MATH_H_ */ diff --git a/src/platforms/ros/geo.cpp b/src/platforms/ros/geo.cpp new file mode 100644 index 000000000..6fad681c9 --- /dev/null +++ b/src/platforms/ros/geo.cpp @@ -0,0 +1,173 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file geo.c + * + * Geo / math functions to perform geodesic calculations + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include <geo/geo.h> +#include <px4.h> +#include <unistd.h> +#include <pthread.h> +#include <stdio.h> +#include <math.h> +#include <stdbool.h> +#include <string.h> +#include <float.h> + +__EXPORT float _wrap_pi(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + + int c = 0; + + while (bearing >= M_PI_F) { + bearing -= M_TWOPI_F; + + if (c++ > 3) { + return NAN; + } + } + + c = 0; + + while (bearing < -M_PI_F) { + bearing += M_TWOPI_F; + + if (c++ > 3) { + return NAN; + } + } + + return bearing; +} + +__EXPORT float _wrap_2pi(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + + int c = 0; + + while (bearing >= M_TWOPI_F) { + bearing -= M_TWOPI_F; + + if (c++ > 3) { + return NAN; + } + } + + c = 0; + + while (bearing < 0.0f) { + bearing += M_TWOPI_F; + + if (c++ > 3) { + return NAN; + } + } + + return bearing; +} + +__EXPORT float _wrap_180(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + + int c = 0; + + while (bearing >= 180.0f) { + bearing -= 360.0f; + + if (c++ > 3) { + return NAN; + } + } + + c = 0; + + while (bearing < -180.0f) { + bearing += 360.0f; + + if (c++ > 3) { + return NAN; + } + } + + return bearing; +} + +__EXPORT float _wrap_360(float bearing) +{ + /* value is inf or NaN */ + if (!isfinite(bearing)) { + return bearing; + } + + int c = 0; + + while (bearing >= 360.0f) { + bearing -= 360.0f; + + if (c++ > 3) { + return NAN; + } + } + + c = 0; + + while (bearing < 0.0f) { + bearing += 360.0f; + + if (c++ > 3) { + return NAN; + } + } + + return bearing; +} diff --git a/src/platforms/ros/nodes/README.md b/src/platforms/ros/nodes/README.md new file mode 100644 index 000000000..aafc647ff --- /dev/null +++ b/src/platforms/ros/nodes/README.md @@ -0,0 +1,22 @@ +# PX4 Nodes + +This directory contains several small ROS nodes which are intended to run alongside the PX4 multi-platform nodes in +ROS. They act as a bridge between the PX4 specific topics and the ROS topics. + +## Joystick Input + +You will need to install the ros joystick packages +See http://wiki.ros.org/joy/Tutorials/ConfiguringALinuxJoystick + +### Arch Linux +```sh +yaourt -Sy ros-indigo-joystick-drivers --noconfirm +``` +check joystick +```sh +ls /dev/input/ +ls -l /dev/input/js0 +``` +(replace 0 by the number you find with the first command) + +make sure the joystick is accessible by the `input` group and that your user is in the `input` group diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp new file mode 100644 index 000000000..bcee0b479 --- /dev/null +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file att_estimator.cpp + * + * @author Thomas Gubler <thomasgubler@gmail.com> + * @author Roman Bapst <romanbapst@yahoo.de> +*/ + +#include "attitude_estimator.h" + +#include <px4/vehicle_attitude.h> +#include <mathlib/mathlib.h> +#include <platforms/px4_defines.h> + +AttitudeEstimator::AttitudeEstimator() : + _n(), + // _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &AttitudeEstimator::ModelStatesCallback, this)), + _sub_imu(_n.subscribe("/px4_multicopter/imu", 1, &AttitudeEstimator::ImuCallback, this)), + _vehicle_attitude_pub(_n.advertise<px4::vehicle_attitude>("vehicle_attitude", 1)) +{ +} + +void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg) +{ + px4::vehicle_attitude msg_v_att; + + /* Fill px4 attitude topic with contents from modelstates topic */ + + /* Convert quaternion to rotation matrix */ + math::Quaternion quat; + //XXX: search for ardrone or other (other than 'plane') vehicle here + int index = 1; + quat(0) = (float)msg->pose[index].orientation.w; + quat(1) = (float)msg->pose[index].orientation.x; + quat(2) = (float) - msg->pose[index].orientation.y; + quat(3) = (float) - msg->pose[index].orientation.z; + + msg_v_att.q[0] = quat(0); + msg_v_att.q[1] = quat(1); + msg_v_att.q[2] = quat(2); + msg_v_att.q[3] = quat(3); + + math::Matrix<3, 3> rot = quat.to_dcm(); + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + PX4_R(msg_v_att.R, i, j) = rot(i, j); + } + } + + msg_v_att.R_valid = true; + + math::Vector<3> euler = rot.to_euler(); + msg_v_att.roll = euler(0); + msg_v_att.pitch = euler(1); + msg_v_att.yaw = euler(2); + + //XXX this is in inertial frame + // msg_v_att.rollspeed = (float)msg->twist[index].angular.x; + // msg_v_att.pitchspeed = -(float)msg->twist[index].angular.y; + // msg_v_att.yawspeed = -(float)msg->twist[index].angular.z; + + _vehicle_attitude_pub.publish(msg_v_att); +} + +void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr &msg) +{ + px4::vehicle_attitude msg_v_att; + + /* Fill px4 attitude topic with contents from modelstates topic */ + + /* Convert quaternion to rotation matrix */ + math::Quaternion quat; + //XXX: search for ardrone or other (other than 'plane') vehicle here + int index = 1; + quat(0) = (float)msg->orientation.w; + quat(1) = (float)msg->orientation.x; + quat(2) = (float) - msg->orientation.y; + quat(3) = (float) - msg->orientation.z; + + msg_v_att.q[0] = quat(0); + msg_v_att.q[1] = quat(1); + msg_v_att.q[2] = quat(2); + msg_v_att.q[3] = quat(3); + + math::Matrix<3, 3> rot = quat.to_dcm(); + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + PX4_R(msg_v_att.R, i, j) = rot(i, j); + } + } + + msg_v_att.R_valid = true; + + math::Vector<3> euler = rot.to_euler(); + msg_v_att.roll = euler(0); + msg_v_att.pitch = euler(1); + msg_v_att.yaw = euler(2); + + msg_v_att.rollspeed = (float)msg->angular_velocity.x; + msg_v_att.pitchspeed = -(float)msg->angular_velocity.y; + msg_v_att.yawspeed = -(float)msg->angular_velocity.z; + + _vehicle_attitude_pub.publish(msg_v_att); +} + + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "attitude_estimator"); + AttitudeEstimator m; + + ros::spin(); + + return 0; +} diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h new file mode 100644 index 000000000..f760a39d8 --- /dev/null +++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file att_estimator.h + * Dummy attitude estimator that forwards attitude from gazebo to px4 topic + * + * @author Thomas Gubler <thomasgubler@gmail.com> +*/ + +#include "ros/ros.h" +#include <gazebo_msgs/ModelStates.h> +#include <sensor_msgs/Imu.h> + +class AttitudeEstimator +{ +public: + AttitudeEstimator(); + + ~AttitudeEstimator() {} + +protected: + void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg); + void ImuCallback(const sensor_msgs::ImuConstPtr &msg); + + ros::NodeHandle _n; + ros::Subscriber _sub_modelstates; + ros::Subscriber _sub_imu; + ros::Publisher _vehicle_attitude_pub; + + +}; diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp new file mode 100644 index 000000000..b9fc296f9 --- /dev/null +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file commander.cpp + * Dummy commander node that publishes the various status topics + * + * @author Thomas Gubler <thomasgubler@gmail.com> +*/ + +#include "commander.h" + +#include <px4/manual_control_setpoint.h> +#include <px4/vehicle_control_mode.h> +#include <px4/vehicle_status.h> +#include <platforms/px4_middleware.h> + +Commander::Commander() : + _n(), + _man_ctrl_sp_sub(_n.subscribe("manual_control_setpoint", 10, &Commander::ManualControlInputCallback, this)), + _vehicle_control_mode_pub(_n.advertise<px4::vehicle_control_mode>("vehicle_control_mode", 10)), + _actuator_armed_pub(_n.advertise<px4::actuator_armed>("actuator_armed", 10)), + _vehicle_status_pub(_n.advertise<px4::vehicle_status>("vehicle_status", 10)), + _parameter_update_pub(_n.advertise<px4::parameter_update>("parameter_update", 10)), + _msg_parameter_update(), + _msg_actuator_armed() +{ +} + +void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg) +{ + px4::vehicle_control_mode msg_vehicle_control_mode; + px4::vehicle_status msg_vehicle_status; + + /* fill vehicle control mode */ + //XXX hardcoded + msg_vehicle_control_mode.timestamp = px4::get_time_micros(); + msg_vehicle_control_mode.flag_control_manual_enabled = true; + msg_vehicle_control_mode.flag_control_rates_enabled = true; + msg_vehicle_control_mode.flag_control_attitude_enabled = true; + + /* fill actuator armed */ + float arm_th = 0.95; + _msg_actuator_armed.timestamp = px4::get_time_micros(); + + if (_msg_actuator_armed.armed) { + /* Check for disarm */ + if (msg->r < -arm_th && msg->z < (1 - arm_th)) { + _msg_actuator_armed.armed = false; + } + + } else { + /* Check for arm */ + if (msg->r > arm_th && msg->z < (1 - arm_th)) { + _msg_actuator_armed.armed = true; + } + } + + /* fill vehicle status */ + //XXX hardcoded + msg_vehicle_status.timestamp = px4::get_time_micros(); + msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_MANUAL; + msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_MANUAL; + msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; + msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF; + msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; + msg_vehicle_status.is_rotary_wing = true; + + _vehicle_control_mode_pub.publish(msg_vehicle_control_mode); + _actuator_armed_pub.publish(_msg_actuator_armed); + _vehicle_status_pub.publish(msg_vehicle_status); + + /* Fill parameter update */ + if (px4::get_time_micros() - _msg_parameter_update.timestamp > 1e6) { + _msg_parameter_update.timestamp = px4::get_time_micros(); + _parameter_update_pub.publish(_msg_parameter_update); + } +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "commander"); + Commander m; + ros::spin(); + return 0; +} diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h new file mode 100644 index 000000000..bd4092b3a --- /dev/null +++ b/src/platforms/ros/nodes/commander/commander.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file commander.h + * Dummy commander node that publishes the various status topics + * + * @author Thomas Gubler <thomasgubler@gmail.com> +*/ + +#include "ros/ros.h" +#include <px4/manual_control_setpoint.h> +#include <px4/parameter_update.h> +#include <px4/actuator_armed.h> + +class Commander +{ +public: + Commander(); + + ~Commander() {} + +protected: + /** + * Based on manual control input the status will be set + */ + void ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg); + + ros::NodeHandle _n; + ros::Subscriber _man_ctrl_sp_sub; + ros::Publisher _vehicle_control_mode_pub; + ros::Publisher _actuator_armed_pub; + ros::Publisher _vehicle_status_pub; + ros::Publisher _parameter_update_pub; + + px4::parameter_update _msg_parameter_update; + px4::actuator_armed _msg_actuator_armed; + +}; diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp new file mode 100644 index 000000000..688df50e0 --- /dev/null +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file manual_input.cpp + * Reads from the ros joystick topic and publishes to the px4 manual control input topic. + * + * @author Thomas Gubler <thomasgubler@gmail.com> +*/ + +#include "manual_input.h" + +#include <px4/manual_control_setpoint.h> +#include <platforms/px4_middleware.h> + +ManualInput::ManualInput() : + _n(), + _joy_sub(_n.subscribe("joy", 1, &ManualInput::JoyCallback, this)), + _man_ctrl_sp_pub(_n.advertise<px4::manual_control_setpoint>("manual_control_setpoint", 1)) +{ + double dz_default = 0.2; + /* Load parameters, default values work for Microsoft XBox Controller */ + _n.param("map_x", _param_axes_map[0], 4); + _n.param("scale_x", _param_axes_scale[0], 1.0); + _n.param("off_x", _param_axes_offset[0], 0.0); + _n.param("dz_x", _param_axes_dz[0], dz_default); + + _n.param("map_y", _param_axes_map[1], 3); + _n.param("scale_y", _param_axes_scale[1], -1.0); + _n.param("off_y", _param_axes_offset[1], 0.0); + _n.param("dz_y", _param_axes_dz[1], dz_default); + + _n.param("map_z", _param_axes_map[2], 2); + _n.param("scale_z", _param_axes_scale[2], -0.5); + _n.param("off_z", _param_axes_offset[2], -1.0); + _n.param("dz_z", _param_axes_dz[2], dz_default); + + _n.param("map_r", _param_axes_map[3], 0); + _n.param("scale_r", _param_axes_scale[3], -1.0); + _n.param("off_r", _param_axes_offset[3], 0.0); + _n.param("dz_r", _param_axes_dz[3], dz_default); + +} + +void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr &msg) +{ + px4::manual_control_setpoint msg_out; + + /* Fill px4 manual control setpoint topic with contents from ros joystick */ + /* Map sticks to x, y, z, r */ + MapAxis(msg, _param_axes_map[0], _param_axes_scale[0], _param_axes_offset[0], _param_axes_dz[0], msg_out.x); + MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], _param_axes_dz[1], msg_out.y); + MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], _param_axes_dz[2], msg_out.z); + MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], _param_axes_dz[3], msg_out.r); + //ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_out.x, msg_out.y, msg_out.z, msg_out.r); + + /* Map buttons to switches */ + //XXX todo + /* for now just publish switches in position for manual flight */ + msg_out.mode_switch = 0; + msg_out.return_switch = 0; + msg_out.posctl_switch = 0; + msg_out.loiter_switch = 0; + msg_out.acro_switch = 0; + msg_out.offboard_switch = 0; + + msg_out.timestamp = px4::get_time_micros(); + + _man_ctrl_sp_pub.publish(msg_out); +} + +void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, + double deadzone, float &out) +{ + double val = msg->axes[map_index]; + + if (val + offset > deadzone || val + offset < -deadzone) { + out = (float)((val + offset)) * scale; + } +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "manual_input"); + ManualInput m; + ros::spin(); + return 0; +} diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h new file mode 100644 index 000000000..93e0abe64 --- /dev/null +++ b/src/platforms/ros/nodes/manual_input/manual_input.h @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file manual_input.h + * Reads from the ros joystick topic and publishes to the px4 manual control setpoint topic. + * + * @author Thomas Gubler <thomasgubler@gmail.com> +*/ + +#include "ros/ros.h" +#include <sensor_msgs/Joy.h> + +class ManualInput +{ +public: + ManualInput(); + + ~ManualInput() {} + +protected: + /** + * Takes ROS joystick message and converts/publishes to px4 manual control setpoint topic + */ + void JoyCallback(const sensor_msgs::JoyConstPtr &msg); + + /** + * Helper function to map and scale joystick input + */ + void MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, double deadzone, + float &out); + + ros::NodeHandle _n; + ros::Subscriber _joy_sub; + ros::Publisher _man_ctrl_sp_pub; + + /* Parameters */ + int _param_axes_map[4]; + double _param_axes_scale[4]; + double _param_axes_offset[4]; + double _param_axes_dz[4]; + + +}; diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp new file mode 100644 index 000000000..54f5fa78b --- /dev/null +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -0,0 +1,276 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_mixer.cpp + * Dummy multicopter mixer for euroc simulator (gazebo) + * + * @author Roman Bapst <romanbapst@yahoo.de> +*/ +#include <ros/ros.h> +#include <px4.h> +#include <lib/mathlib/mathlib.h> +#include <mav_msgs/MotorSpeed.h> +#include <string> + +class MultirotorMixer +{ +public: + + MultirotorMixer(); + + struct Rotor { + float roll_scale; + float pitch_scale; + float yaw_scale; + }; + + void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg); + void actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg); + +private: + + ros::NodeHandle _n; + ros::Subscriber _sub; + ros::Publisher _pub; + + const Rotor *_rotors; + + unsigned _rotor_count; + + struct { + float control[8]; + } inputs; + + struct { + float control[8]; + } outputs; + + bool _armed; + ros::Subscriber _sub_actuator_armed; + + void mix(); +}; + +const MultirotorMixer::Rotor _config_x[] = { + { -0.707107, 0.707107, 1.00 }, + { 0.707107, -0.707107, 1.00 }, + { 0.707107, 0.707107, -1.00 }, + { -0.707107, -0.707107, -1.00 }, +}; + +const MultirotorMixer::Rotor _config_quad_plus[] = { + { -1.000000, 0.000000, 1.00 }, + { 1.000000, 0.000000, 1.00 }, + { 0.000000, 1.000000, -1.00 }, + { -0.000000, -1.000000, -1.00 }, +}; + +const MultirotorMixer::Rotor _config_quad_plus_euroc[] = { + { 0.000000, 1.000000, 1.00 }, + { -0.000000, -1.000000, 1.00 }, + { 1.000000, 0.000000, -1.00 }, + { -1.000000, 0.000000, -1.00 }, +}; +const MultirotorMixer::Rotor _config_quad_wide[] = { + { -0.927184, 0.374607, 1.000000 }, + { 0.777146, -0.629320, 1.000000 }, + { 0.927184, 0.374607, -1.000000 }, + { -0.777146, -0.629320, -1.000000 }, +}; +const MultirotorMixer::Rotor _config_quad_iris[] = { + { -0.876559, 0.481295, 1.000000 }, + { 0.826590, -0.562805, 1.000000 }, + { 0.876559, 0.481295, -1.000000 }, + { -0.826590, -0.562805, -1.000000 }, +}; + +const MultirotorMixer::Rotor *_config_index[5] = { + &_config_x[0], + &_config_quad_plus[0], + &_config_quad_plus_euroc[0], + &_config_quad_wide[0], + &_config_quad_iris[0] +}; + +MultirotorMixer::MultirotorMixer(): + _n(), + _rotor_count(4), + _rotors(_config_index[0]) +{ + _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this); + _pub = _n.advertise<mav_msgs::MotorSpeed>("/mixed_motor_commands", 10); + + if (!_n.hasParam("motor_scaling_radps")) { + _n.setParam("motor_scaling_radps", 150.0); + } + + if (!_n.hasParam("motor_offset_radps")) { + _n.setParam("motor_offset_radps", 600.0); + } + + if (!_n.hasParam("mixer")) { + _n.setParam("mixer", "x"); + } + + _sub_actuator_armed = _n.subscribe("actuator_armed", 1, &MultirotorMixer::actuatorArmedCallback, this); +} + +void MultirotorMixer::mix() +{ + float roll = math::constrain(inputs.control[0], -1.0f, 1.0f); + float pitch = math::constrain(inputs.control[1], -1.0f, 1.0f); + float yaw = math::constrain(inputs.control[2], -1.0f, 1.0f); + float thrust = math::constrain(inputs.control[3], 0.0f, 1.0f); + float min_out = 0.0f; + float max_out = 0.0f; + + /* perform initial mix pass yielding unbounded outputs, ignore yaw */ + for (unsigned i = 0; i < _rotor_count; i++) { + float out = roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale + thrust; + + /* limit yaw if it causes outputs clipping */ + if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { + yaw = -out / _rotors[i].yaw_scale; + } + + /* calculate min and max output values */ + if (out < min_out) { + min_out = out; + } + + if (out > max_out) { + max_out = out; + } + + outputs.control[i] = out; + } + + /* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */ + if (min_out < 0.0f) { + float scale_in = thrust / (thrust - min_out); + + /* mix again with adjusted controls */ + for (unsigned i = 0; i < _rotor_count; i++) { + outputs.control[i] = scale_in + * (roll * _rotors[i].roll_scale + + pitch * _rotors[i].pitch_scale) + thrust; + } + + } else { + /* roll/pitch mixed without limiting, add yaw control */ + for (unsigned i = 0; i < _rotor_count; i++) { + outputs.control[i] += yaw * _rotors[i].yaw_scale; + } + } + + /* scale down all outputs if some outputs are too large, reduce total thrust */ + float scale_out; + + if (max_out > 1.0f) { + scale_out = 1.0f / max_out; + + } else { + scale_out = 1.0f; + } + + /* scale outputs to range _idle_speed..1, and do final limiting */ + for (unsigned i = 0; i < _rotor_count; i++) { + outputs.control[i] = math::constrain(outputs.control[i], 0.0f, 1.0f); + } +} + +void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg) +{ + // read message + for (int i = 0; i < msg.NUM_ACTUATOR_CONTROLS; i++) { + inputs.control[i] = msg.control[i]; + } + + // switch mixer if necessary + std::string mixer_name; + _n.getParamCached("mixer", mixer_name); + if (mixer_name == "x") { + _rotors = _config_index[0]; + ROS_WARN("using x"); + } else if (mixer_name == "+") { + _rotors = _config_index[1]; + } else if (mixer_name == "e") { + _rotors = _config_index[2]; + } else if (mixer_name == "w") { + _rotors = _config_index[3]; + ROS_WARN("using w"); + } else if (mixer_name == "i") { + _rotors = _config_index[4]; + ROS_WARN("using i"); + } + ROS_WARN("mixer_name %s", mixer_name.c_str()); + + // mix + mix(); + + // publish message + mav_msgs::MotorSpeed rotor_vel_msg; + double scaling; + double offset; + _n.getParamCached("motor_scaling_radps", scaling); + _n.getParamCached("motor_offset_radps", offset); + + if (_armed) { + for (int i = 0; i < _rotor_count; i++) { + rotor_vel_msg.motor_speed.push_back(outputs.control[i] * scaling + offset); + } + + } else { + for (int i = 0; i < _rotor_count; i++) { + rotor_vel_msg.motor_speed.push_back(0.0); + } + } + + _pub.publish(rotor_vel_msg); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "mc_mixer"); + MultirotorMixer mixer; + ros::spin(); + + return 0; +} + +void MultirotorMixer::actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg) +{ + _armed = msg.armed; +} diff --git a/src/platforms/ros/perf_counter.cpp b/src/platforms/ros/perf_counter.cpp new file mode 100755 index 000000000..a71801397 --- /dev/null +++ b/src/platforms/ros/perf_counter.cpp @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * Copyright (C) 2012 - 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file perf_counter.cpp + * + * Empty function calls for ros compatibility + * + * @author Roman Bapst <bapstr@ethz.ch> + * + */ +#include <stdlib.h> +#include <stdio.h> +#include <systemlib/perf_counter.h> + + + +perf_counter_t perf_alloc(enum perf_counter_type type, const char *name) +{ + return NULL; +} + +/** + * Free a counter. + * + * @param handle The performance counter's handle. + */ +void perf_free(perf_counter_t handle) +{ + +} + +/** + * Count a performance event. + * + * This call only affects counters that take single events; PC_COUNT, PC_INTERVAL etc. + * + * @param handle The handle returned from perf_alloc. + */ +void perf_count(perf_counter_t handle) +{ + +} + +/** + * Begin a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * + * @param handle The handle returned from perf_alloc. + */ +void perf_begin(perf_counter_t handle) +{ + +} + +/** + * End a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * If a call is made without a corresopnding perf_begin call, or if perf_cancel + * has been called subsequently, no change is made to the counter. + * + * @param handle The handle returned from perf_alloc. + */ +void perf_end(perf_counter_t handle) +{ + +} + +/** + * Cancel a performance event. + * + * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. + * It reverts the effect of a previous perf_begin. + * + * @param handle The handle returned from perf_alloc. + */ +void perf_cancel(perf_counter_t handle) +{ + +} + +/** + * Reset a performance counter. + * + * This call resets performance counter to initial state + * + * @param handle The handle returned from perf_alloc. + */ +void perf_reset(perf_counter_t handle) +{ + +} + +/** + * Print one performance counter to stdout + * + * @param handle The counter to print. + */ +void perf_print_counter(perf_counter_t handle) +{ + +} + +/** + * Print one performance counter to a fd. + * + * @param fd File descriptor to print to - e.g. 0 for stdout + * @param handle The counter to print. + */ +void perf_print_counter_fd(int fd, perf_counter_t handle) +{ + +} + +/** + * Print all of the performance counters. + * + * @param fd File descriptor to print to - e.g. 0 for stdout + */ +void perf_print_all(int fd) +{ + +} + +/** + * Reset all of the performance counters. + */ +void perf_reset_all(void) +{ + +} + +/** + * Return current event_count + * + * @param handle The counter returned from perf_alloc. + * @return event_count + */ +uint64_t perf_event_count(perf_counter_t handle) +{ + +} + + diff --git a/src/platforms/ros/px4_nodehandle.cpp b/src/platforms/ros/px4_nodehandle.cpp new file mode 100644 index 000000000..6ac3c76d3 --- /dev/null +++ b/src/platforms/ros/px4_nodehandle.cpp @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4_nodehandle.cpp + * + * PX4 Middleware Wrapper Nodehandle + */ +#include <px4_nodehandle.h> + +namespace px4 +{ +} + diff --git a/src/platforms/ros/px4_publisher.cpp b/src/platforms/ros/px4_publisher.cpp new file mode 100644 index 000000000..f02dbe4c9 --- /dev/null +++ b/src/platforms/ros/px4_publisher.cpp @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4_publisher.cpp + * + * PX4 Middleware Wrapper for Publisher + */ +#include <px4_publisher.h> + + diff --git a/src/platforms/ros/px4_ros_impl.cpp b/src/platforms/ros/px4_ros_impl.cpp new file mode 100644 index 000000000..854986a7f --- /dev/null +++ b/src/platforms/ros/px4_ros_impl.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4_ros_impl.cpp + * + * PX4 Middleware Wrapper ROS Implementation + */ + +#include <px4.h> + +namespace px4 +{ + +void init(int argc, char *argv[], const char *process_name) +{ + ros::init(argc, argv, process_name); +} + +uint64_t get_time_micros() +{ + ros::Time time = ros::Time::now(); + return time.sec * 1e6 + time.nsec / 1000; +} + +} |