Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-20 11:23:06 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

platforms delete PX4_ROS and obsolete headers

Browse Source
This commit is contained in:
Daniel Agar
2019-01-22 11:04:13 -05:00
parent 67e5986c9b
commit 376e078c24
36 changed files with 9 additions and 3280 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/drivers/device/nuttx/SPI.hpp
-2
View File
@@ -42,8 +42,6 @@
#include "../CDev.hpp"
#include <px4_spi.h>
namespace device __EXPORT
{
src/lib/drivers/device/posix/SPI.hpp
-2
View File
@@ -42,8 +42,6 @@
#include "CDev.hpp"
#include <px4_spi.h>
namespace device __EXPORT
{
src/platforms/px4_app.h
-13
View File
@@ -48,12 +48,6 @@ class AppState
public:
~AppState() {}
#if defined(__PX4_ROS)
AppState() {}
bool exitRequested() { return !ros::ok(); }
void requestExit() { ros::shutdown(); }
#else
AppState() : _exitRequested(false), _isRunning(false) {}
bool exitRequested() { return _exitRequested; }
@@ -65,7 +59,6 @@ public:
protected:
bool _exitRequested;
bool _isRunning;
#endif
private:
AppState(const AppState &);
const AppState &operator=(const AppState &);
@@ -73,15 +66,9 @@ private:
}
// Task/process based build
#if defined(__PX4_ROS)
// Thread based build
#else
#ifdef PX4_MAIN
extern int PX4_MAIN(int argc, char *argv[]);
#endif
#endif
src/platforms/px4_defines.h
+3 -19
View File
@@ -72,23 +72,7 @@
#define PX4_ISFINITE(x) std::isfinite(x)
#endif
#if defined(__PX4_ROS)
/****************************************************************************
* Building for running within the ROS environment.
****************************************************************************/
#define noreturn_function
#ifdef __cplusplus
#include "ros/ros.h"
#endif
/* Main entry point */
#define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv)
/* Get value of parameter by name, which is equal to the handle for ros */
#define PX4_PARAM_GET_BYNAME(_name, _destpt) ros::param::get(_name, *_destpt)
#elif defined(__PX4_NUTTX) || defined(__PX4_POSIX)
#if defined(__PX4_NUTTX) || defined(__PX4_POSIX)
/****************************************************************************
* Building for NuttX or POSIX.
****************************************************************************/
@@ -209,7 +193,7 @@ __END_DECLS
#define PX4_STORAGEDIR PX4_ROOTFSDIR
#endif // __PX4_POSIX
#if defined(__PX4_ROS) || defined(__PX4_POSIX)
#if defined(__PX4_POSIX)
/****************************************************************************
* Defines for POSIX and ROS
****************************************************************************/
@@ -248,4 +232,4 @@ __END_DECLS
#define M_DEG_TO_RAD 0.017453292519943295
#define M_RAD_TO_DEG 57.295779513082323
#endif // defined(__PX4_ROS) || defined(__PX4_POSIX)
#endif // defined(__PX4_POSIX)
src/platforms/px4_i2c.h
+1 -5
View File
@@ -41,11 +41,7 @@
#define PX4_I2C_M_READ 0x0001 /* read data, from slave to master */
#if defined(__PX4_ROS)
#error "Devices not supported in ROS"
#elif defined (__PX4_NUTTX)
#if defined (__PX4_NUTTX)
__BEGIN_DECLS
/*
src/platforms/px4_includes.h
+1 -6
View File
@@ -39,12 +39,7 @@
#pragma once
#if defined(__PX4_ROS)
/*
* Building for running within the ROS environment
*/
#elif defined(__PX4_NUTTX)
#if defined(__PX4_NUTTX)
/*
* Building for NuttX
*/
src/platforms/px4_log.h
+1 -12
View File
@@ -66,18 +66,7 @@ __END_DECLS
****************************************************************************/
#define __px4_log_omit(level, FMT, ...) do_nothing(level, ##__VA_ARGS__)
#if defined(__PX4_ROS)
#include <ros/console.h>
#define PX4_PANIC(...) ROS_FATAL(__VA_ARGS__)
#define PX4_ERR(...) ROS_ERROR(__VA_ARGS__)
#define PX4_WARN(...) ROS_WARN(__VA_ARGS__)
#define PX4_INFO(...) ROS_INFO(__VA_ARGS__)
#define PX4_INFO_RAW(...) printf(__VA_ARGS__)
#define PX4_DEBUG(...) ROS_DEBUG(__VA_ARGS__)
#define PX4_BACKTRACE()
#elif defined(__PX4_QURT)
#if defined(__PX4_QURT)
#include "qurt_log.h"
/****************************************************************************
* Messages that should never be filtered or compiled out
src/platforms/px4_message.h
-77
View File
@@ -1,77 +0,0 @@
/****************************************************************************
*
* 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_message.h
*
* Defines the message base types
*/
#pragma once
#if defined(__PX4_ROS)
typedef const char *PX4TopicHandle;
#else
#include <uORB/uORB.h>
typedef orb_id_t PX4TopicHandle;
#endif
namespace px4
{
template <typename M>
class __EXPORT PX4Message
{
// friend class NodeHandle;
// #if defined(__PX4_ROS)
// template<typename T>
// friend class SubscriberROS;
// #endif
public:
PX4Message() :
_data()
{}
PX4Message(M msg) :
_data(msg)
{}
virtual ~PX4Message() {}
virtual M &data() {return _data;}
virtual const M &data() const {return _data;}
private:
M _data;
};
}
src/platforms/px4_middleware.h
+1 -6
View File
@@ -49,12 +49,7 @@ __EXPORT void init(int argc, char *argv[], const char *process_name);
__EXPORT uint64_t get_time_micros();
#if defined(__PX4_ROS)
/**
* Returns true if the app/task should continue to run
*/
inline bool ok() { return ros::ok(); }
#elif defined(__PX4_NUTTX)
#if defined(__PX4_NUTTX)
extern bool task_should_exit;
/**
* Returns true if the app/task should continue to run
src/platforms/px4_nodehandle.h
-312
View File
@@ -1,312 +0,0 @@
/****************************************************************************
*
* 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.h
*
* PX4 Middleware Wrapper Node Handle
*/
#pragma once
/* includes for all platforms */
#include "px4_time.h"
#include "px4_subscriber.h"
#include "px4_publisher.h"
#include "px4_middleware.h"
#include "px4_app.h"
#if defined(__PX4_ROS)
/* includes when building for ros */
#include "ros/ros.h"
#include <list>
#include <inttypes.h>
#include <type_traits>
#else
/* includes when building for NuttX */
#include <px4_posix.h>
#include <poll.h>
#endif
#include <functional>
namespace px4
{
#if defined(__PX4_ROS)
class NodeHandle :
private ros::NodeHandle
{
public:
NodeHandle(AppState &a) :
ros::NodeHandle(),
_subs(),
_pubs(),
_appState(a)
{}
~NodeHandle()
{
_subs.clear();
_pubs.clear();
};
/**
* Subscribe with callback to function
* @param topic Name of the topic
* @param fb Callback, executed on receiving a new message
*/
template<typename T>
Subscriber<T> *subscribe(void(*fp)(const T &), unsigned interval)
{
SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle *)this, std::bind(fp, std::placeholders::_1));
_subs.push_back(sub);
return (Subscriber<T> *)sub;
}
/**
* Subscribe with callback to class method
* @param fb Callback, executed on receiving a new message
* @param obj pointer class instance
*/
template<typename T, typename C>
Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval)
{
SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle *)this, std::bind(fp, obj, std::placeholders::_1));
_subs.push_back(sub);
return (Subscriber<T> *)sub;
}
/**
* Subscribe with no callback, just the latest value is stored on updates
*/
template<typename T>
Subscriber<T> *subscribe(unsigned interval)
{
SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle *)this);
_subs.push_back(sub);
return (Subscriber<T> *)sub;
}
/**
* Advertise topic
*/
template<typename T>
Publisher<T> *advertise()
{
PublisherROS<T> *pub = new PublisherROS<T>((ros::NodeHandle *)this);
_pubs.push_back((PublisherBase *)pub);
return (Publisher<T> *)pub;
}
/**
* Calls all callback waiting to be called
*/
void spinOnce() { ros::spinOnce(); }
/**
* Keeps calling callbacks for incoming messages, returns when module is terminated
*/
void spin() { ros::spin(); }
protected:
std::list<SubscriberBase *> _subs; /**< Subcriptions of node */
std::list<PublisherBase *> _pubs; /**< Publications of node */
AppState &_appState;
};
#else //Building for NuttX
class __EXPORT NodeHandle
{
public:
NodeHandle(AppState &a) :
_subs(),
_pubs(),
_sub_min_interval(nullptr),
_appState(a)
{}
~NodeHandle()
{
/* Empty subscriptions list */
SubscriberNode *sub = _subs.getHead();
int count = 0;
while (sub != nullptr) {
if (count++ > kMaxSubscriptions) {
PX4_WARN("exceeded max subscriptions");
break;
}
SubscriberNode *sib = sub->getSibling();
delete sub;
sub = sib;
}
/* Empty publications list */
PublisherNode *pub = _pubs.getHead();
count = 0;
while (pub != nullptr) {
if (count++ > kMaxPublications) {
PX4_WARN("exceeded max publications");
break;
}
PublisherNode *sib = pub->getSibling();
delete pub;
pub = sib;
}
};
/**
* Subscribe with callback to function
* @param fp Callback, executed on receiving a new message
* @param interval Minimal interval between calls to callback
*/
template<typename T>
Subscriber<T> *subscribe(void(*fp)(const T &), unsigned interval)
{
(void)interval;
SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(interval, std::bind(fp, std::placeholders::_1));
update_sub_min_interval(interval, sub_px4);
_subs.add((SubscriberNode *)sub_px4);
return (Subscriber<T> *)sub_px4;
}
/**
* Subscribe with callback to class method
* @param fb Callback, executed on receiving a new message
* @param obj pointer class instance
*/
template<typename T, typename C>
Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval)
{
(void)interval;
SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(interval, std::bind(fp, obj, std::placeholders::_1));
update_sub_min_interval(interval, sub_px4);
_subs.add((SubscriberNode *)sub_px4);
return (Subscriber<T> *)sub_px4;
}
/**
* Subscribe without callback to function
* @param interval Minimal interval between data fetches from orb
*/
template<typename T>
Subscriber<T> *subscribe(unsigned interval)
{
(void)interval;
SubscriberUORB<T> *sub_px4 = new SubscriberUORB<T>(interval);
update_sub_min_interval(interval, sub_px4);
_subs.add((SubscriberNode *)sub_px4);
return (Subscriber<T> *)sub_px4;
}
/**
* Advertise topic
*/
template<typename T>
Publisher<T> *advertise()
{
PublisherUORB<T> *pub = new PublisherUORB<T>();
_pubs.add(pub);
return (Publisher<T> *)pub;
}
/**
* Calls all callback waiting to be called
*/
void spinOnce()
{
/* Loop through subscriptions, call callback for updated subscriptions */
SubscriberNode *sub = _subs.getHead();
int count = 0;
while (sub != nullptr) {
if (count++ > kMaxSubscriptions) {
PX4_WARN("exceeded max subscriptions");
break;
}
sub->update();
sub = sub->getSibling();
}
}
/**
* Keeps calling callbacks for incoming messages, returns when module is terminated
*/
void spin()
{
while (!_appState.exitRequested()) {
const int timeout_ms = 100;
/* Only continue in the loop if the nodehandle has subscriptions */
if (_sub_min_interval == nullptr) {
px4_usleep(timeout_ms * 1000);
continue;
}
/* Poll fd with smallest interval */
px4_pollfd_struct_t pfd;
pfd.fd = _sub_min_interval->getUORBHandle();
pfd.events = POLLIN;
px4_poll(&pfd, 1, timeout_ms);
spinOnce();
}
}
protected:
static const uint16_t kMaxSubscriptions = 100;
static const uint16_t kMaxPublications = 100;
List<SubscriberNode *> _subs; /**< Subcriptions of node */
List<PublisherNode *> _pubs; /**< Publications of node */
SubscriberNode *_sub_min_interval; /**< Points to the sub with the smallest interval
of all Subscriptions in _subs*/
AppState &_appState;
/**
* Check if this is the smallest interval so far and update _sub_min_interval
*/
template<typename T>
void update_sub_min_interval(unsigned interval, SubscriberUORB<T> *sub)
{
if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
_sub_min_interval = sub;
}
}
};
#endif
}
src/platforms/px4_publisher.h
-163
View File
@@ -1,163 +0,0 @@
/****************************************************************************
*
* 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.h
*
* PX4 Publisher API, implements publishing of messages from a nodehandle
*/
#pragma once
#if defined(__PX4_ROS)
/* includes when building for ros */
#include "ros/ros.h"
#else
/* includes when building for NuttX */
#include <uORB/Publication.hpp>
#include <containers/List.hpp>
#endif
#include <platforms/px4_message.h>
namespace px4
{
/**
* Untemplated publisher base class
* */
class __EXPORT PublisherBase
{
public:
PublisherBase() {}
~PublisherBase() {}
};
/**
* Publisher base class, templated with the message type
* */
template <typename T>
class __EXPORT Publisher
{
public:
Publisher() {}
~Publisher() {}
virtual int publish(const T &msg) = 0;
};
#if defined(__PX4_ROS)
template <typename T>
class PublisherROS :
public Publisher<T>
{
public:
/**
* Construct Publisher by providing a ros::Publisher
* @param ros_pub the ros publisher which will be used to perform the publications
*/
PublisherROS(ros::NodeHandle *rnh) :
Publisher<T>(),
_ros_pub(rnh->advertise < typename std::remove_reference < decltype(((T *)nullptr)->data()) >::type & > (T::handle(),
kQueueSizeDefault))
{}
~PublisherROS() {}
/** Publishes msg
* @param msg the message which is published to the topic
*/
int publish(const T &msg)
{
_ros_pub.publish(msg.data());
return 0;
}
protected:
static const uint32_t kQueueSizeDefault = 1; /**< Size of queue for ROS */
ros::Publisher _ros_pub; /**< Handle to the ros publisher */
};
#else
/**
* Because we maintain a list of publishers we need a node class
*/
class __EXPORT PublisherNode :
public ListNode<PublisherNode *>
{
public:
PublisherNode() :
ListNode()
{}
virtual ~PublisherNode() {}
virtual void update() = 0;
};
template <typename T>
class __EXPORT PublisherUORB :
public Publisher<T>,
public PublisherNode
{
public:
/**
* Construct Publisher by providing orb meta data
*/
PublisherUORB() :
Publisher<T>(),
PublisherNode(),
_uorb_pub(new uORB::PublicationBase(T::handle()))
{}
~PublisherUORB()
{
delete _uorb_pub;
};
/** Publishes msg
* @param msg the message which is published to the topic
*/
int publish(const T &msg)
{
_uorb_pub->update((void *) & (msg.data()));
return 0;
}
/**
* Empty callback for list traversal
*/
void update() {} ;
private:
uORB::PublicationBase *_uorb_pub; /**< Handle to the publisher */
};
#endif
}
src/platforms/px4_spi.h
-33
View File
@@ -1,33 +0,0 @@
#pragma once
#ifdef __PX4_NUTTX
#include <nuttx/config.h>
#include <nuttx/spi/spi.h>
#elif defined(__PX4_POSIX)
enum spi_dev_e {
SPIDEV_NONE = 0, /* Not a valid value */
SPIDEV_MMCSD, /* Select SPI MMC/SD device */
SPIDEV_FLASH, /* Select SPI FLASH device */
SPIDEV_ETHERNET, /* Select SPI ethernet device */
SPIDEV_DISPLAY, /* Select SPI LCD/OLED display device */
SPIDEV_WIRELESS, /* Select SPI Wireless device */
SPIDEV_TOUCHSCREEN, /* Select SPI touchscreen device */
SPIDEV_EXPANDER, /* Select SPI I/O expander device */
SPIDEV_MUX, /* Select SPI multiplexer device */
SPIDEV_AUDIO_DATA, /* Select SPI audio codec device data port */
SPIDEV_AUDIO_CTRL /* Select SPI audio codec device control port */
};
/* Certain SPI devices may required different clocking modes */
enum spi_mode_e {
SPIDEV_MODE0 = 0, /* CPOL=0 CHPHA=0 */
SPIDEV_MODE1, /* CPOL=0 CHPHA=1 */
SPIDEV_MODE2, /* CPOL=1 CHPHA=0 */
SPIDEV_MODE3 /* CPOL=1 CHPHA=1 */
};
struct spi_dev_s {
int unused;
};
#else
#endif
src/platforms/px4_subscriber.h
-296
View File
@@ -1,296 +0,0 @@
/****************************************************************************
*
* 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_subscriber.h
*
* PX4 Subscriber API, implements subscribing to messages from a nodehandle
*/
#pragma once
#ifndef CONFIG_ARCH_BOARD_SIM
#include <functional>
#include <type_traits>
#endif
#if defined(__PX4_ROS)
/* includes when building for ros */
#include "ros/ros.h"
#else
/* includes when building for NuttX */
#include <uORB/Subscription.hpp>
#include <containers/List.hpp>
#include "px4_nodehandle.h"
#endif
namespace px4
{
/**
* Untemplated subscriber base class
* */
class __EXPORT SubscriberBase
{
public:
SubscriberBase() {}
virtual ~SubscriberBase() {}
};
/**
* Subscriber class which is used by nodehandle
*/
template<typename T>
class __EXPORT Subscriber :
public SubscriberBase
{
public:
Subscriber() :
SubscriberBase(),
_msg_current()
{}
virtual ~Subscriber() {}
/* Accessors*/
/**
* Get the last message value
*/
virtual T &get() {return _msg_current;}
/**
* Get the last native message value
*/
virtual decltype(((T *)nullptr)->data()) data()
{
return _msg_current.data();
}
protected:
T _msg_current; /**< Current Message value */
};
#if defined(__PX4_ROS)
/**
* Subscriber class that is templated with the ros n message type
*/
template<typename T>
class SubscriberROS :
public Subscriber<T>
{
public:
/**
* Construct Subscriber without a callback function
*/
SubscriberROS(ros::NodeHandle *rnh) :
px4::Subscriber<T>(),
_cbf(NULL),
_ros_sub(rnh->subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS<T>::callback, this))
{}
/**
* Construct Subscriber by providing a callback function
*/
SubscriberROS(ros::NodeHandle *rnh, std::function<void(const T &)> cbf) :
_cbf(cbf),
_ros_sub(rnh->subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS<T>::callback, this))
{}
virtual ~SubscriberROS() {}
protected:
static const uint32_t kQueueSizeDefault = 1; /**< Size of queue for ROS */
/**
* Called on topic update, saves the current message and then calls the provided callback function
* needs to use the native type as it is called by ROS
*/
void callback(const typename std::remove_reference < decltype(((T *)nullptr)->data()) >::type &msg)
{
/* Store data */
this->_msg_current.data() = msg;
/* Call callback */
if (_cbf != NULL) {
_cbf(this->get());
}
}
ros::Subscriber _ros_sub; /**< Handle to ros subscriber */
std::function<void(const T &)> _cbf; /**< Callback that the user provided on the subscription */
};
#else // Building for NuttX
/**
* Because we maintain a list of subscribers we need a node class
*/
class __EXPORT SubscriberNode :
public ListNode<SubscriberNode *>
{
public:
SubscriberNode(unsigned interval) :
ListNode(),
_interval(interval)
{}
virtual ~SubscriberNode() {}
virtual void update() = 0;
virtual int getUORBHandle() = 0;
unsigned get_interval() { return _interval; }
protected:
unsigned _interval;
};
/**
* Subscriber class that is templated with the uorb subscription message type
*/
template<typename T>
class __EXPORT SubscriberUORB :
public Subscriber<T>,
public SubscriberNode
{
public:
/**
* Construct SubscriberUORB by providing orb meta data without callback
* @param interval Minimal interval between calls to callback
*/
SubscriberUORB(unsigned interval) :
SubscriberNode(interval),
_uorb_sub(new uORB::SubscriptionBase(T::handle(), interval))
{}
virtual ~SubscriberUORB()
{
delete _uorb_sub;
};
/**
* Update Subscription
* Invoked by the list traversal in NodeHandle::spinOnce
*/
virtual void update()
{
if (!_uorb_sub->updated()) {
/* Topic not updated, do not call callback */
return;
}
_uorb_sub->update(get_void_ptr());
};
/* Accessors*/
int getUORBHandle() { return _uorb_sub->getHandle(); }
protected:
uORB::SubscriptionBase *_uorb_sub; /**< Handle to the subscription */
#ifndef CONFIG_ARCH_BOARD_SIM
typename std::remove_reference < decltype(((T *)nullptr)->data()) >::type getUORBData()
{
return (typename std::remove_reference < decltype(((T *)nullptr)->data()) >::type) * _uorb_sub;
}
#endif
/**
* Get void pointer to last message value
*/
void *get_void_ptr() { return (void *) & (this->_msg_current.data()); }
};
//XXX reduce to one class with overloaded constructor?
template<typename T>
class __EXPORT SubscriberUORBCallback :
public SubscriberUORB<T>
{
public:
/**
* Construct SubscriberUORBCallback by providing orb meta data
* @param cbf Callback, executed on receiving a new message
* @param interval Minimal interval between calls to callback
*/
SubscriberUORBCallback(unsigned interval
#ifndef CONFIG_ARCH_BOARD_SIM
, std::function<void(const T &)> cbf)
#else
)
#endif
:
SubscriberUORB<T>(interval),
_cbf(cbf)
{}
virtual ~SubscriberUORBCallback() {}
/**
* Update Subscription
* Invoked by the list traversal in NodeHandle::spinOnce
* If new data is available the callback is called
*/
virtual void update()
{
if (!this->_uorb_sub->updated()) {
/* Topic not updated, do not call callback */
return;
}
/* get latest data */
this->_uorb_sub->update(this->get_void_ptr());
/* Check if there is a callback */
if (_cbf == nullptr) {
return;
}
/* Call callback which performs actions based on this data */
_cbf(Subscriber<T>::get());
};
protected:
#ifndef CONFIG_ARCH_BOARD_SIM
std::function<void(const T &)> _cbf; /**< Callback that the user provided on the subscription */
#endif
};
#endif
}
src/platforms/px4_tasks.h
+1 -5
View File
@@ -44,11 +44,7 @@
#include <stdbool.h>
#ifdef __PX4_ROS
#error "PX4 tasks not supported in ROS"
#elif defined(__PX4_NUTTX)
#if defined(__PX4_NUTTX)
typedef int px4_task_t;
#include <sys/prctl.h>
src/platforms/px4_workqueue.h
+1 -3
View File
@@ -35,9 +35,7 @@
#pragma once
#if defined(__PX4_ROS)
#error "Work queue not supported on ROS"
#elif defined(__PX4_NUTTX)
#if defined(__PX4_NUTTX)
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
src/platforms/ros/eigen_math.h
-19
View File
@@ -1,19 +0,0 @@
/*
* 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_ */
src/platforms/ros/nodes/README.md
-22
View File
@@ -1,22 +0,0 @@
# 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
src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
-124
View File
@@ -1,124 +0,0 @@
/****************************************************************************
*
* 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>
#include <platforms/px4_middleware.h>
AttitudeEstimator::AttitudeEstimator() :
_n(),
// _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &AttitudeEstimator::ModelStatesCallback, this)),
_vehicle_attitude_pub(_n.advertise<px4::vehicle_attitude>("vehicle_attitude", 1))
{
std::string vehicle_model;
_n.param("vehicle_model", vehicle_model, std::string("iris"));
_sub_imu = _n.subscribe("/" + vehicle_model + "/imu", 1, &AttitudeEstimator::ImuCallback, this);
}
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);
//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;
msg_v_att.timestamp = px4::get_time_micros();
_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);
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;
msg_v_att.timestamp = px4::get_time_micros();
_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;
}
src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h
-64
View File
@@ -1,64 +0,0 @@
/****************************************************************************
*
* 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>
*/
#pragma once
#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;
};
src/platforms/ros/nodes/commander/commander.cpp
-240
View File
@@ -1,240 +0,0 @@
/****************************************************************************
*
* 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 <platforms/px4_middleware.h>
Commander::Commander() :
_n(),
_man_ctrl_sp_sub(_n.subscribe("manual_control_setpoint", 10, &Commander::ManualControlInputCallback, this)),
_offboard_control_mode_sub(_n.subscribe("offboard_control_mode", 10, &Commander::OffboardControlModeCallback, 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)),
_commander_state_pub(_n.advertise<px4::commander_state>("commander_state", 10)),
_parameter_update_pub(_n.advertise<px4::parameter_update>("parameter_update", 10)),
_msg_parameter_update(),
_msg_actuator_armed(),
_msg_vehicle_control_mode(),
_msg_vehicle_status(),
_msg_offboard_control_mode(),
_got_manual_control(false)
{
/* Default to offboard control: when no joystick is connected offboard control should just work */
}
void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg)
{
_got_manual_control = true;
/* fill vehicle control mode based on (faked) stick positions*/
EvalSwitches(msg, _msg_vehicle_status, _msg_commander_state, _msg_vehicle_control_mode);
_msg_vehicle_control_mode.timestamp = px4::get_time_micros();
/* 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;
_msg_vehicle_control_mode.flag_armed = false;
_msg_vehicle_status.arming_state = _msg_vehicle_status.ARMING_STATE_STANDBY;
}
} else {
/* Check for arm */
if (msg->r > arm_th && msg->z < (1 - arm_th)) {
_msg_actuator_armed.armed = true;
_msg_vehicle_control_mode.flag_armed = true;
_msg_vehicle_status.arming_state = _msg_vehicle_status.ARMING_STATE_ARMED;
}
}
/* fill vehicle status */
_msg_vehicle_status.timestamp = px4::get_time_micros();
_msg_vehicle_status.hil_state = _msg_vehicle_status.HIL_STATE_OFF;
_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);
_commander_state_pub.publish(_msg_commander_state);
/* 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);
}
}
void Commander::SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode,
px4::vehicle_control_mode &msg_vehicle_control_mode)
{
msg_vehicle_control_mode.flag_control_manual_enabled = false;
msg_vehicle_control_mode.flag_control_offboard_enabled = true;
msg_vehicle_control_mode.flag_control_auto_enabled = false;
msg_vehicle_control_mode.flag_control_rates_enabled = !msg_offboard_control_mode.ignore_bodyrate ||
!msg_offboard_control_mode.ignore_attitude ||
!msg_offboard_control_mode.ignore_position ||
!msg_offboard_control_mode.ignore_velocity ||
!msg_offboard_control_mode.ignore_acceleration_force;
msg_vehicle_control_mode.flag_control_attitude_enabled = !msg_offboard_control_mode.ignore_attitude ||
!msg_offboard_control_mode.ignore_position ||
!msg_offboard_control_mode.ignore_velocity ||
!msg_offboard_control_mode.ignore_acceleration_force;
msg_vehicle_control_mode.flag_control_velocity_enabled = !msg_offboard_control_mode.ignore_velocity ||
!msg_offboard_control_mode.ignore_position;
msg_vehicle_control_mode.flag_control_climb_rate_enabled = !msg_offboard_control_mode.ignore_velocity ||
!msg_offboard_control_mode.ignore_position;
msg_vehicle_control_mode.flag_control_position_enabled = !msg_offboard_control_mode.ignore_position;
msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_velocity ||
!msg_offboard_control_mode.ignore_position;
}
void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
px4::vehicle_status &msg_vehicle_status,
px4::commander_state &msg_commander_state,
px4::vehicle_control_mode &msg_vehicle_control_mode)
{
// XXX this is a minimal implementation. If more advanced functionalities are
// needed consider a full port of the commander
if (msg->offboard_switch == px4::manual_control_setpoint::SWITCH_POS_ON) {
SetOffboardControl(_msg_offboard_control_mode, msg_vehicle_control_mode);
return;
}
msg_vehicle_control_mode.flag_control_offboard_enabled = false;
switch (msg->mode_switch) {
case px4::manual_control_setpoint::SWITCH_POS_NONE:
ROS_WARN("Joystick button mapping error, main mode not set");
break;
case px4::manual_control_setpoint::SWITCH_POS_OFF: // MANUAL
msg_commander_state.main_state = msg_commander_state.MAIN_STATE_MANUAL;
msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_MANUAL;
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;
msg_vehicle_control_mode.flag_control_altitude_enabled = false;
msg_vehicle_control_mode.flag_control_climb_rate_enabled = false;
msg_vehicle_control_mode.flag_control_position_enabled = false;
msg_vehicle_control_mode.flag_control_velocity_enabled = false;
break;
case px4::manual_control_setpoint::SWITCH_POS_MIDDLE: // ASSIST
if (msg->posctl_switch == px4::manual_control_setpoint::SWITCH_POS_ON) {
msg_commander_state.main_state = msg_commander_state.MAIN_STATE_POSCTL;
msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_POSCTL;
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;
msg_vehicle_control_mode.flag_control_altitude_enabled = true;
msg_vehicle_control_mode.flag_control_climb_rate_enabled = true;
msg_vehicle_control_mode.flag_control_position_enabled = true;
msg_vehicle_control_mode.flag_control_velocity_enabled = true;
} else {
msg_commander_state.main_state = msg_commander_state.MAIN_STATE_ALTCTL;
msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_ALTCTL;
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;
msg_vehicle_control_mode.flag_control_altitude_enabled = true;
msg_vehicle_control_mode.flag_control_climb_rate_enabled = true;
msg_vehicle_control_mode.flag_control_position_enabled = false;
msg_vehicle_control_mode.flag_control_velocity_enabled = false;
}
break;
}
}
void Commander::OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &msg)
{
_msg_offboard_control_mode = *msg;
/* Force system into offboard control mode */
if (!_got_manual_control) {
SetOffboardControl(_msg_offboard_control_mode, _msg_vehicle_control_mode);
_msg_vehicle_status.timestamp = px4::get_time_micros();
_msg_vehicle_status.hil_state = _msg_vehicle_status.HIL_STATE_OFF;
_msg_vehicle_status.is_rotary_wing = true;
_msg_vehicle_status.arming_state = _msg_vehicle_status.ARMING_STATE_ARMED;
_msg_actuator_armed.armed = true;
_msg_actuator_armed.timestamp = px4::get_time_micros();
_msg_vehicle_control_mode.timestamp = px4::get_time_micros();
_msg_vehicle_control_mode.flag_armed = true;
_vehicle_control_mode_pub.publish(_msg_vehicle_control_mode);
_actuator_armed_pub.publish(_msg_actuator_armed);
_vehicle_status_pub.publish(_msg_vehicle_status);
_commander_state_pub.publish(_msg_vehicle_status);
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "commander");
Commander m;
ros::spin();
return 0;
}
src/platforms/ros/nodes/commander/commander.h
-102
View File
@@ -1,102 +0,0 @@
/****************************************************************************
*
* 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>
*/
#pragma once
#include "ros/ros.h"
#include <px4/manual_control_setpoint.h>
#include <px4/vehicle_control_mode.h>
#include <px4/vehicle_status.h>
#include <px4/commander_state.h>
#include <px4/parameter_update.h>
#include <px4/actuator_armed.h>
#include <px4/offboard_control_mode.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);
/**
* Stores the offboard control mode
*/
void OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &msg);
/**
* Set control mode flags based on stick positions (equiv to code in px4 commander)
*/
void EvalSwitches(const px4::manual_control_setpointConstPtr &msg,
px4::vehicle_status &msg_vehicle_status,
px4::commander_state &msg_commander_state,
px4::vehicle_control_mode &msg_vehicle_control_mode);
/**
* Sets offboard control flags in msg_vehicle_control_mode
*/
void SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode,
px4::vehicle_control_mode &msg_vehicle_control_mode);
ros::NodeHandle _n;
ros::Subscriber _man_ctrl_sp_sub;
ros::Subscriber _offboard_control_mode_sub;
ros::Publisher _vehicle_control_mode_pub;
ros::Publisher _actuator_armed_pub;
ros::Publisher _vehicle_status_pub;
ros::Publisher _commander_state_pub;
ros::Publisher _parameter_update_pub;
px4::parameter_update _msg_parameter_update;
px4::actuator_armed _msg_actuator_armed;
px4::vehicle_control_mode _msg_vehicle_control_mode;
px4::vehicle_status _msg_vehicle_status;
px4::commander_state _msg_commander_state;
px4::offboard_control_mode _msg_offboard_control_mode;
bool _got_manual_control;
};
src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp
-88
View File
@@ -1,88 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 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 demo_offboard_position_Setpoints.cpp
*
* Demo for sending offboard position setpoints to mavros to show offboard position control in SITL
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "demo_offboard_attitude_setpoints.h"
#include <platforms/px4_middleware.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/Float64.h>
#include <math.h>
#include <tf/transform_datatypes.h>
DemoOffboardAttitudeSetpoints::DemoOffboardAttitudeSetpoints() :
_n(),
_attitude_sp_pub(_n.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_attitude/attitude", 1)),
_thrust_sp_pub(_n.advertise<std_msgs::Float64>("mavros/setpoint_attitude/att_throttle", 1))
{
}
int DemoOffboardAttitudeSetpoints::main()
{
px4::Rate loop_rate(10);
while (ros::ok()) {
loop_rate.sleep();
ros::spinOnce();
/* Publish example offboard attitude setpoint */
geometry_msgs::PoseStamped pose;
tf::Quaternion q = tf::createQuaternionFromRPY(0.0, 0.1 * (sinf(0.5 * (float)px4::get_time_micros() / 1000000.0f)),
0.0);
quaternionTFToMsg(q, pose.pose.orientation);
_attitude_sp_pub.publish(pose);
std_msgs::Float64 thrust;
thrust.data = 0.4f + 0.25 * (sinf((float)px4::get_time_micros() /
1000000.0f)); // just some example throttle input that makes the quad 'jump'
_thrust_sp_pub.publish(thrust);
}
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "demo_offboard_position_setpoints");
DemoOffboardAttitudeSetpoints d;
return d.main();
}
src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.h
-60
View File
@@ -1,60 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 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 demo_offboard_attitude_Setpoints.h
*
* Demo for sending offboard position setpoints to mavros to show offboard position control in SITL
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#pragma once
#include "ros/ros.h"
#include <px4/manual_control_setpoint.h>
class DemoOffboardAttitudeSetpoints
{
public:
DemoOffboardAttitudeSetpoints();
~DemoOffboardAttitudeSetpoints() {}
int main();
protected:
ros::NodeHandle _n;
ros::Publisher _attitude_sp_pub;
ros::Publisher _thrust_sp_pub;
};
src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp
-78
View File
@@ -1,78 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 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 demo_offboard_position_Setpoints.cpp
*
* Demo for sending offboard position setpoints to mavros to show offboard position control in SITL
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "demo_offboard_position_setpoints.h"
#include <platforms/px4_middleware.h>
#include <geometry_msgs/PoseStamped.h>
DemoOffboardPositionSetpoints::DemoOffboardPositionSetpoints() :
_n(),
_local_position_sp_pub(_n.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 1))
{
}
int DemoOffboardPositionSetpoints::main()
{
px4::Rate loop_rate(10);
while (ros::ok()) {
loop_rate.sleep();
ros::spinOnce();
/* Publish example offboard position setpoint */
geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 1;
_local_position_sp_pub.publish(pose);
}
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "demo_offboard_position_setpoints");
DemoOffboardPositionSetpoints d;
return d.main();
}
src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h
-59
View File
@@ -1,59 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 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 demo_offboard_position_Setpoints.h
*
* Demo for sending offboard position setpoints to mavros to show offboard position control in SITL
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#pragma once
#include "ros/ros.h"
#include <px4/manual_control_setpoint.h>
class DemoOffboardPositionSetpoints
{
public:
DemoOffboardPositionSetpoints();
~DemoOffboardPositionSetpoints() {}
int main();
protected:
ros::NodeHandle _n;
ros::Publisher _local_position_sp_pub;
};
src/platforms/ros/nodes/manual_input/manual_input.cpp
-174
View File
@@ -1,174 +0,0 @@
/****************************************************************************
*
* 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 <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)),
_buttons_state(),
_msg_mc_sp()
{
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], 0.0);
_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);
_n.param("map_manual", _param_buttons_map[0], 0);
_n.param("map_altctl", _param_buttons_map[1], 1);
_n.param("map_posctl", _param_buttons_map[2], 2);
_n.param("map_auto_mission", _param_buttons_map[3], 3);
_n.param("map_auto_loiter", _param_buttons_map[4], 4);
_n.param("map_auto_rtl", _param_buttons_map[5], 5);
_n.param("map_offboard", _param_buttons_map[6], 6);
/* Default to manual */
_msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
_msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
_msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
_msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
_msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
_msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
_msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE;
}
void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr &msg)
{
/* 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_mc_sp.x);
MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], _param_axes_dz[1], _msg_mc_sp.y);
MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], _param_axes_dz[2], _msg_mc_sp.z);
MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], _param_axes_dz[3], _msg_mc_sp.r);
//ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_mc_sp.x, msg_out.y, msg_out.z, msg_out.r);
/* Map buttons to switches */
MapButtons(msg, _msg_mc_sp);
_msg_mc_sp.timestamp = px4::get_time_micros();
_man_ctrl_sp_pub.publish(_msg_mc_sp);
}
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;
} else {
out = 0.0f;
}
}
void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp)
{
msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE;
if (_buttons_state[MAIN_STATE_MANUAL] != msg->buttons[_param_buttons_map[MAIN_STATE_MANUAL]] == true) {
msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
return;
} else if (_buttons_state[MAIN_STATE_ALTCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_ALTCTL]] == true) {
msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE;
msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
return;
} else if (_buttons_state[MAIN_STATE_POSCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_POSCTL]] == true) {
msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE;
msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_ON;
msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
return;
} else if (_buttons_state[MAIN_STATE_OFFBOARD] != msg->buttons[_param_buttons_map[MAIN_STATE_OFFBOARD]] == true) {
msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE;
msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_ON;
msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF;
msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_ON;
return;
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "manual_input");
ManualInput m;
ros::spin();
return 0;
}
src/platforms/ros/nodes/manual_input/manual_input.h
-97
View File
@@ -1,97 +0,0 @@
/****************************************************************************
*
* 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>
*/
#pragma once
#include "ros/ros.h"
#include <px4/manual_control_setpoint.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 axis
*/
void MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, double deadzone,
float &out);
/**
* Helper function to map joystick buttons
*/
void MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp);
ros::NodeHandle _n;
ros::Subscriber _joy_sub;
ros::Publisher _man_ctrl_sp_pub;
/* Parameters */
enum MAIN_STATE {
MAIN_STATE_MANUAL = 0,
MAIN_STATE_ALTCTL,
MAIN_STATE_POSCTL,
MAIN_STATE_AUTO_MISSION,
MAIN_STATE_AUTO_LOITER,
MAIN_STATE_AUTO_RTL,
// MAIN_STATE_ACRO,
MAIN_STATE_OFFBOARD,
MAIN_STATE_MAX
};
int _param_buttons_map[MAIN_STATE_MAX]; /**< joystick button mapping, order according to MAIN_STATE */
bool _buttons_state[MAIN_STATE_MAX]; /**< joystick button state of last iteration,
order according to MAIN_STATE */
int _param_axes_map[4];
double _param_axes_scale[4];
double _param_axes_offset[4];
double _param_axes_dz[4];
px4::manual_control_setpoint _msg_mc_sp;
};
src/platforms/ros/nodes/mavlink/mavlink.cpp
-305
View File
@@ -1,305 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 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 mavlink.cpp
* Dummy mavlink node that interfaces to a mavros node via UDP
* This simulates the onboard mavlink app to some degree. It should be possible to
* send offboard setpoints via mavros to the SITL setup the same way as on the real system
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "mavlink.h"
#include <platforms/px4_middleware.h>
using namespace px4;
Mavlink::Mavlink(std::string mavlink_fcu_url) :
_n(),
_v_att_sub(_n.subscribe("vehicle_attitude", 1, &Mavlink::VehicleAttitudeCallback, this)),
_v_local_pos_sub(_n.subscribe("vehicle_local_position", 1, &Mavlink::VehicleLocalPositionCallback, this)),
_v_att_sp_pub(_n.advertise<vehicle_attitude_setpoint>("vehicle_attitude_setpoint", 1)),
_pos_sp_triplet_pub(_n.advertise<position_setpoint_triplet>("position_setpoint_triplet", 1)),
_offboard_control_mode_pub(_n.advertise<offboard_control_mode>("offboard_control_mode", 1))
{
_link = mavconn::MAVConnInterface::open_url(mavlink_fcu_url);
_link->message_received.connect(boost::bind(&Mavlink::handle_msg, this, _1, _2, _3));
_att_sp = {};
_offboard_control_mode = {};
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "mavlink");
ros::NodeHandle privateNh("~");
std::string mavlink_fcu_url;
privateNh.param<std::string>("mavlink_fcu_url", mavlink_fcu_url,
std::string("udp://localhost:14565@localhost:14560"));
Mavlink m(mavlink_fcu_url);
ros::spin();
return 0;
}
void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg)
{
mavlink_message_t msg_m;
mavlink_msg_attitude_quaternion_pack_chan(
_link->get_system_id(),
_link->get_component_id(),
_link->get_channel(),
&msg_m,
get_time_micros() / 1000,
msg->q[0],
msg->q[1],
msg->q[2],
msg->q[3],
msg->rollspeed,
msg->pitchspeed,
msg->yawspeed);
_link->send_message(&msg_m);
}
void Mavlink::VehicleLocalPositionCallback(const vehicle_local_positionConstPtr &msg)
{
mavlink_message_t msg_m;
mavlink_msg_local_position_ned_pack_chan(
_link->get_system_id(),
_link->get_component_id(),
_link->get_channel(),
&msg_m,
get_time_micros() / 1000,
msg->x,
msg->y,
msg->z,
msg->vx,
msg->vy,
msg->vz);
_link->send_message(&msg_m);
}
void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid)
{
(void)sysid;
(void)compid;
switch (mmsg->msgid) {
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
handle_msg_set_attitude_target(mmsg);
break;
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
handle_msg_set_position_target_local_ned(mmsg);
break;
default:
break;
}
}
void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg)
{
mavlink_set_attitude_target_t set_attitude_target;
mavlink_msg_set_attitude_target_decode(mmsg, &set_attitude_target);
/* set correct ignore flags for thrust field: copy from mavlink message */
_offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6));
/*
* if attitude or body rate have been used (not ignored) previously and this message only sends
* throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and
* body rates to keep the controllers running
*/
bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7);
bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7));
if (ignore_bodyrate && ignore_attitude && !_offboard_control_mode.ignore_thrust) {
/* Message want's us to ignore everything except thrust: only ignore if previously ignored */
_offboard_control_mode.ignore_bodyrate = ignore_bodyrate && _offboard_control_mode.ignore_bodyrate;
_offboard_control_mode.ignore_attitude = ignore_attitude && _offboard_control_mode.ignore_attitude;
} else {
_offboard_control_mode.ignore_bodyrate = ignore_bodyrate;
_offboard_control_mode.ignore_attitude = ignore_attitude;
}
_offboard_control_mode.ignore_position = true;
_offboard_control_mode.ignore_velocity = true;
_offboard_control_mode.ignore_acceleration_force = true;
_offboard_control_mode.timestamp = get_time_micros();
_offboard_control_mode_pub.publish(_offboard_control_mode);
/* The real mavlink app has a ckeck at this location which makes sure that the attitude setpoint
* gets published only if in offboard mode. We leave that out for now.
*/
_att_sp.timestamp = get_time_micros();
if (!ignore_attitude) {
mavlink_quaternion_to_euler(set_attitude_target.q, &_att_sp.roll_body, &_att_sp.pitch_body,
&_att_sp.yaw_body);
mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])_att_sp.R_body.data());
_att_sp.R_valid = true;
}
if (!_offboard_control_mode.ignore_thrust) {
_att_sp.thrust = set_attitude_target.thrust;
}
if (!ignore_attitude) {
for (ssize_t i = 0; i < 4; i++) {
_att_sp.q_d[i] = set_attitude_target.q[i];
}
_att_sp.q_d_valid = true;
}
_v_att_sp_pub.publish(_att_sp);
//XXX real mavlink publishes rate sp here
}
void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t *mmsg)
{
mavlink_set_position_target_local_ned_t set_position_target_local_ned;
mavlink_msg_set_position_target_local_ned_decode(mmsg, &set_position_target_local_ned);
offboard_control_mode offboard_control_mode;
// memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints
/* Only accept messages which are intended for this system */
// XXX removed for sitl, makes maybe sense to re-introduce at some point
// if ((mavlink_system.sysid == set_position_target_local_ned.target_system ||
// set_position_target_local_ned.target_system == 0) &&
// (mavlink_system.compid == set_position_target_local_ned.target_component ||
// set_position_target_local_ned.target_component == 0)) {
/* convert mavlink type (local, NED) to uORB offboard control struct */
offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7);
offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38);
offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0);
bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
/* yaw ignore flag mapps to ignore_attitude */
offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400);
/* yawrate ignore flag mapps to ignore_bodyrate */
offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800);
offboard_control_mode.timestamp = get_time_micros();
_offboard_control_mode_pub.publish(offboard_control_mode);
/* The real mavlink app has a ckeck at this location which makes sure that the position setpoint triplet
* gets published only if in offboard mode. We leave that out for now.
*/
if (is_force_sp && offboard_control_mode.ignore_position &&
offboard_control_mode.ignore_velocity) {
/* The offboard setpoint is a force setpoint only, directly writing to the force
* setpoint topic and not publishing the setpoint triplet topic */
PX4_WARN("force setpoint not supported");
} else {
/* It's not a pure force setpoint: publish to setpoint triplet topic */
position_setpoint_triplet pos_sp_triplet;
pos_sp_triplet.previous.valid = false;
pos_sp_triplet.next.valid = false;
pos_sp_triplet.current.valid = true;
pos_sp_triplet.current.type = position_setpoint::SETPOINT_TYPE_POSITION; //XXX support others
/* set the local pos values */
if (!offboard_control_mode.ignore_position) {
pos_sp_triplet.current.position_valid = true;
pos_sp_triplet.current.x = set_position_target_local_ned.x;
pos_sp_triplet.current.y = set_position_target_local_ned.y;
pos_sp_triplet.current.z = set_position_target_local_ned.z;
} else {
pos_sp_triplet.current.position_valid = false;
}
/* set the local vel values */
if (!offboard_control_mode.ignore_velocity) {
pos_sp_triplet.current.velocity_valid = true;
pos_sp_triplet.current.vx = set_position_target_local_ned.vx;
pos_sp_triplet.current.vy = set_position_target_local_ned.vy;
pos_sp_triplet.current.vz = set_position_target_local_ned.vz;
} else {
pos_sp_triplet.current.velocity_valid = false;
}
/* set the local acceleration values if the setpoint type is 'local pos' and none
* of the accelerations fields is set to 'ignore' */
if (!offboard_control_mode.ignore_acceleration_force) {
pos_sp_triplet.current.acceleration_valid = true;
pos_sp_triplet.current.a_x = set_position_target_local_ned.afx;
pos_sp_triplet.current.a_y = set_position_target_local_ned.afy;
pos_sp_triplet.current.a_z = set_position_target_local_ned.afz;
pos_sp_triplet.current.acceleration_is_force =
is_force_sp;
} else {
pos_sp_triplet.current.acceleration_valid = false;
}
/* set the yaw sp value */
if (!offboard_control_mode.ignore_attitude && !isnan(set_position_target_local_ned.yaw)) {
pos_sp_triplet.current.yaw_valid = true;
pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw;
} else {
pos_sp_triplet.current.yaw_valid = false;
}
/* set the yawrate sp value */
if (!offboard_control_mode.ignore_bodyrate && !isnan(set_position_target_local_ned.yaw)) {
pos_sp_triplet.current.yawspeed_valid = true;
pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;
} else {
pos_sp_triplet.current.yawspeed_valid = false;
}
//XXX handle global pos setpoints (different MAV frames)
_pos_sp_triplet_pub.publish(pos_sp_triplet);
}
}
src/platforms/ros/nodes/mavlink/mavlink.h
-116
View File
@@ -1,116 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 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 mavlink.h
* Dummy mavlink node that interfaces to a mavros node via UDP
* This simulates the onboard mavlink app to some degree. It should be possible to
* send offboard setpoints via mavros to the SITL setup the same way as on the real system
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#pragma once
#include "ros/ros.h"
#include <mavconn/interface.h>
#include <px4/vehicle_attitude.h>
#include <px4/vehicle_local_position.h>
#include <px4/vehicle_attitude_setpoint.h>
#include <px4/position_setpoint_triplet.h>
#include <px4/offboard_control_mode.h>
namespace px4
{
class Mavlink
{
public:
Mavlink(std::string mavlink_fcu_url);
~Mavlink() {}
protected:
ros::NodeHandle _n;
mavconn::MAVConnInterface::Ptr _link;
ros::Subscriber _v_att_sub;
ros::Subscriber _v_local_pos_sub;
ros::Publisher _v_att_sp_pub;
ros::Publisher _pos_sp_triplet_pub;
ros::Publisher _offboard_control_mode_pub;
ros::Publisher _force_sp_pub;
vehicle_attitude_setpoint _att_sp;
offboard_control_mode _offboard_control_mode;
/**
*
* Simulates output of attitude data from the FCU
* Equivalent to the mavlink stream ATTITUDE_QUATERNION
*
* */
void VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg);
/**
*
* Simulates output of local position data from the FCU
* Equivalent to the mavlink stream LOCAL_POSITION_NED
*
* */
void VehicleLocalPositionCallback(const vehicle_local_positionConstPtr &msg);
/**
*
* Handle incoming mavlink messages ant publish them to ROS ("Mavlink Receiver")
*
* */
void handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid);
/**
*
* Handle SET_ATTITUDE_TARGET mavlink messages
*
* */
void handle_msg_set_attitude_target(const mavlink_message_t *mmsg);
/**
*
* Handle SET_POSITION_TARGET_LOCAL_NED mavlink messages
*
* */
void handle_msg_set_position_target_local_ned(const mavlink_message_t *mmsg);
};
}
src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
-277
View File
@@ -1,277 +0,0 @@
/****************************************************************************
*
* 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 rotors simulator (gazebo)
*
* @author Roman Bapst <romanbapst@yahoo.de>
*/
#include <ros/ros.h>
#include <px4.h>
#include <lib/mathlib/mathlib.h>
#include <mav_msgs/CommandMotorSpeed.h>
#include <string>
class MultirotorMixer
{
public:
MultirotorMixer();
struct Rotor {
float roll_scale;
float pitch_scale;
float yaw_scale;
};
void actuatorControlsCallback(const px4::actuator_controls_0 &msg);
void actuatorArmedCallback(const px4::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_rotorssim[] = {
{ 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_rotorssim[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::CommandMotorSpeed>("command/motor_speed", 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::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];
} 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];
} else if (mixer_name == "i") {
_rotors = _config_index[4];
}
// mix
mix();
// publish message
mav_msgs::CommandMotorSpeed 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::actuator_armed &msg)
{
_armed = msg.armed;
}
src/platforms/ros/nodes/position_estimator/position_estimator.cpp
-112
View File
@@ -1,112 +0,0 @@
/****************************************************************************
*
* 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 position_estimator.cpp
*
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Roman Bapst <romanbapst@yahoo.de>
*/
#include "position_estimator.h"
#include <px4/vehicle_local_position.h>
#include <mathlib/mathlib.h>
#include <platforms/px4_defines.h>
#include <platforms/px4_middleware.h>
#include <vector>
#include <string>
#include <gazebo_msgs/ModelStates.h>
PositionEstimator::PositionEstimator() :
_n(),
_sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &PositionEstimator::ModelStatesCallback, this)),
_vehicle_position_pub(_n.advertise<px4::vehicle_local_position>("vehicle_local_position", 1)),
_startup_time(1)
{
_n.getParam("vehicle_model", _model_name);
}
void PositionEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg)
{
//XXX: use a proper sensor topic
px4::vehicle_local_position msg_v_l_pos;
/* Fill px4 position topic with contents from modelstates topic */
int index = 0;
//XXX: maybe a more clever approach would be to do this not on every loop, need to check if and when
//gazebo rearranges indexes.
for (std::vector<std::string>::const_iterator it = msg->name.begin(); it != msg->name.end(); ++it) {
if (*it == _model_name) {
index = it - msg->name.begin();
break;
}
}
msg_v_l_pos.xy_valid = true;
msg_v_l_pos.z_valid = true;
msg_v_l_pos.v_xy_valid = true;
msg_v_l_pos.v_z_valid = true;
msg_v_l_pos.x = msg->pose[index].position.x;
msg_v_l_pos.y = -msg->pose[index].position.y;
msg_v_l_pos.z = -msg->pose[index].position.z;
msg_v_l_pos.vx = msg->twist[index].linear.x;
msg_v_l_pos.vy = -msg->twist[index].linear.y;
msg_v_l_pos.vz = -msg->twist[index].linear.z;
msg_v_l_pos.xy_global = true;
msg_v_l_pos.z_global = true;
msg_v_l_pos.ref_timestamp = _startup_time;
msg_v_l_pos.ref_lat = 47.378301;
msg_v_l_pos.ref_lon = 8.538777;
msg_v_l_pos.ref_alt = 1200.0f;
msg_v_l_pos.vxy_max = 0.0f;
msg_v_l_pos.limit_hagl = false;
msg_v_l_pos.timestamp = px4::get_time_micros();
_vehicle_position_pub.publish(msg_v_l_pos);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "position_estimator");
PositionEstimator m;
ros::spin();
return 0;
}
src/platforms/ros/nodes/position_estimator/position_estimator.h
-64
View File
@@ -1,64 +0,0 @@
/****************************************************************************
*
* 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 position_estimator.h
* Dummy position estimator that forwards position from gazebo to px4 topic
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#pragma once
#include "ros/ros.h"
#include <gazebo_msgs/ModelStates.h>
#include <sensor_msgs/Imu.h>
class PositionEstimator
{
public:
PositionEstimator();
~PositionEstimator() {}
protected:
void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg);
ros::NodeHandle _n;
ros::Subscriber _sub_modelstates;
ros::Publisher _vehicle_position_pub;
uint64_t _startup_time;
std::string _model_name;
};
src/platforms/ros/perf_counter.cpp
-184
View File
@@ -1,184 +0,0 @@
/****************************************************************************
*
* 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 <perf/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)
{
}
int perf_print_counter_buffer(char *buffer, int length, perf_counter_t handle)
{
return 0;
}
void perf_iterate_all(perf_callback cb, void *user)
{
}
/**
* 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)
{
return 0;
}
src/platforms/ros/px4_nodehandle.cpp
-44
View File
@@ -1,44 +0,0 @@
/****************************************************************************
*
* 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
{
}
src/platforms/ros/px4_publisher.cpp
-41
View File
@@ -1,41 +0,0 @@
/****************************************************************************
*
* 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>
src/platforms/ros/px4_ros_impl.cpp
-56
View File
@@ -1,56 +0,0 @@
/****************************************************************************
*
* 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;
}
}