-
Notifications
You must be signed in to change notification settings - Fork 90
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Feature/ros publishers #103
Changes from all commits
6c19875
1834af6
c43e8dc
82a14c6
57c6878
c3e6e60
3c2d073
a61145a
527b254
e4c5a5f
9862f2b
05e331d
b61cf78
ccc0f28
6c4b965
3035a1e
b354b41
1fd8030
24f11d4
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,50 @@ | ||
#ifndef PIKSI_MULTI_CPP_CALLBACK_H_ | ||
#define PIKSI_MULTI_CPP_CALLBACK_H_ | ||
|
||
#include <libsbp/sbp.h> | ||
#include <ros/ros.h> | ||
#include <memory> | ||
|
||
namespace piksi_multi_cpp { | ||
|
||
const uint32_t kQueueSize = 100; | ||
const bool kLatchTopic = true; | ||
|
||
// This class handles the callback creation for the different piksi message | ||
// types. | ||
class Callback { | ||
public: | ||
// Register callback. | ||
Callback(const ros::NodeHandle& nh, const uint16_t sbp_msg_type, | ||
const std::shared_ptr<sbp_state_t>& state); | ||
|
||
// Factory method to create callbacks. | ||
// Add new message types here. | ||
static std::shared_ptr<Callback> create( | ||
const ros::NodeHandle& nh, const uint16_t sbp_msg_type, | ||
const std::shared_ptr<sbp_state_t>& state); | ||
|
||
protected: | ||
// Implement the specific callback here. | ||
virtual void callback(uint16_t sender_id, uint8_t len, uint8_t msg[]) = 0; | ||
// Every callback has at least one default publisher that relays the current | ||
// message. | ||
ros::Publisher relay_pub_; | ||
// A nodehandle with the correct ROS namespace. | ||
ros::NodeHandle nh_; | ||
|
||
private: | ||
// This function will be passed to sbp_register_callback. | ||
// libsbp is a C interface and does not allow binding a non-static member | ||
// function using std::bind. Thus we bind this static member function that | ||
// points on the context's callbackSBP function. | ||
static void callback_redirect(uint16_t sender_id, uint8_t len, uint8_t msg[], | ||
void* context); | ||
|
||
sbp_msg_callbacks_node_t sbp_msg_callback_node_; | ||
std::shared_ptr<sbp_state_t> state_; | ||
}; | ||
|
||
} // namespace piksi_multi_cpp | ||
|
||
#endif // PIKSI_MULTI_CPP_CALLBACK_H_ |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,21 @@ | ||
#ifndef PIKSI_MULTI_CPP_CALLBACK_HEARTBEAT_H_ | ||
#define PIKSI_MULTI_CPP_CALLBACK_HEARTBEAT_H_ | ||
|
||
#include <libsbp/sbp.h> | ||
#include <ros/ros.h> | ||
#include "piksi_multi_cpp/callback.h" | ||
|
||
namespace piksi_multi_cpp { | ||
|
||
class CallbackHeartbeat : public Callback { | ||
public: | ||
CallbackHeartbeat(const ros::NodeHandle& nh, const uint16_t sbp_msg_type, | ||
const std::shared_ptr<sbp_state_t>& state); | ||
|
||
private: | ||
void callback(uint16_t sender_id, uint8_t len, uint8_t msg[]) override; | ||
}; | ||
|
||
} // namespace piksi_multi_cpp | ||
|
||
#endif // PIKSI_MULTI_CPP_CALLBACK_HEARTBEAT_H_ |
This file was deleted.
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,86 @@ | ||
#ifndef PIKSI_MULTI_CPP_RECEIVER_H_ | ||
#define PIKSI_MULTI_CPP_RECEIVER_H_ | ||
|
||
#include <libsbp/sbp.h> | ||
#include <ros/ros.h> | ||
#include <memory> | ||
#include <string> | ||
#include <vector> | ||
#include "piksi_multi_cpp/callback.h" | ||
#include "piksi_multi_cpp/device.h" | ||
|
||
namespace piksi_multi_cpp { | ||
|
||
// This class offers a ROS interface for Piksi Multi receivers. It automatically | ||
// assigns a type to a device and offers ROS topics and services. | ||
class Receiver { | ||
public: | ||
/* The three types of receivers are | ||
|
||
kBaseStationReceiver: The static base station sending out RTK corrections. | ||
|
||
kPositionReceiver: The moving rover receiving RTK corrections from the | ||
base station and broadcasting RTK GPS positions. | ||
|
||
kAttitudeReceiver: The moving rover receiving RTK corrections from a moving | ||
reference receiver and broadcasting the moving baseline (also referred to as | ||
heading). */ | ||
enum ReceiverType { | ||
kBaseStationReceiver = 0, | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Here there is also quite some ambiguity - we have an enum for this, and a class hierarchy. I would do away with the enum and use the class hierarchy only. Any other class using these types doesn't need to know which it is, otherwise it breaks the some separation-of-concern principles |
||
kPositionReceiver, | ||
kAttitudeReceiver, | ||
kUnknown | ||
}; | ||
static std::vector<ReceiverType> kTypeVec; | ||
|
||
Receiver(const ros::NodeHandle& nh, const std::shared_ptr<Device>& device); | ||
|
||
// Closes device. | ||
~Receiver(); | ||
|
||
// Open device. | ||
bool init(); | ||
// Read device and process SBP callbacks. | ||
void process(); | ||
|
||
// Factory methods to create receivers. | ||
|
||
// Create receiver by setting node handle, hardware device, and type. | ||
// Warning: Node handle namespace must be unique for every device. | ||
static std::shared_ptr<Receiver> create(const ros::NodeHandle& nh, | ||
const std::shared_ptr<Device>& device, | ||
const ReceiverType type); | ||
|
||
// Create receiver from node handle and hardware device. Auto infer type from | ||
// device. | ||
// Warning: Node handle namespace must be unique for every device. | ||
static std::shared_ptr<Receiver> create( | ||
const ros::NodeHandle& nh, const std::shared_ptr<Device>& device); | ||
|
||
// Create all receivers from node handle only. Autodetects connected hardware | ||
// devices, infers device type from Piksi firmware settings and assigns unique | ||
// name spaces. | ||
static std::vector<std::shared_ptr<Receiver>> createAllReceivers( | ||
const ros::NodeHandle& nh); | ||
|
||
protected: | ||
// ROS node handle in the correct receiver namespace. | ||
ros::NodeHandle nh_; | ||
// The actual hardware interface. | ||
std::shared_ptr<Device> device_; | ||
|
||
private: | ||
// Infer receiver type from Piksi firmware settings. | ||
static ReceiverType inferType(const std::shared_ptr<Device>& dev); | ||
static std::string createNameSpace(const ReceiverType type, const size_t id); | ||
|
||
// The sbp state. | ||
std::shared_ptr<sbp_state_t> state_; | ||
|
||
// SBP callbacks common for all receivers. | ||
std::vector<std::shared_ptr<Callback>> cb_; | ||
}; | ||
|
||
} // namespace piksi_multi_cpp | ||
|
||
#endif // PIKSI_MULTI_CPP_RECEIVER_H_ |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,23 @@ | ||
#ifndef PIKSI_MULTI_CPP_RECEIVER_ATTITUDE_H_ | ||
#define PIKSI_MULTI_CPP_RECEIVER_ATTITUDE_H_ | ||
|
||
#include <ros/ros.h> | ||
#include <string> | ||
#include "piksi_multi_cpp/device.h" | ||
#include "piksi_multi_cpp/receiver.h" | ||
|
||
// This class creates ROS publishers for attitude receivers. See also | ||
// https://support.swiftnav.com/customer/en/portal/articles/2805901-piksi-multi---heading | ||
namespace piksi_multi_cpp { | ||
|
||
class ReceiverAttitude : public Receiver { | ||
public: | ||
ReceiverAttitude(const ros::NodeHandle& nh, | ||
const std::shared_ptr<Device>& device); | ||
|
||
private: | ||
}; | ||
|
||
} // namespace piksi_multi_cpp | ||
|
||
#endif // PIKSI_MULTI_CPP_RECEIVER_ATTITUDE_H_ |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,21 @@ | ||
#ifndef PIKSI_MULTI_CPP_RECEIVER_BASE_STATION_H_ | ||
#define PIKSI_MULTI_CPP_RECEIVER_BASE_STATION_H_ | ||
|
||
#include <ros/ros.h> | ||
#include <string> | ||
#include "piksi_multi_cpp/device.h" | ||
#include "piksi_multi_cpp/receiver.h" | ||
|
||
namespace piksi_multi_cpp { | ||
|
||
class ReceiverBaseStation : public Receiver { | ||
public: | ||
ReceiverBaseStation(const ros::NodeHandle& nh, | ||
const std::shared_ptr<Device>& device); | ||
|
||
private: | ||
}; | ||
|
||
} // namespace piksi_multi_cpp | ||
|
||
#endif // PIKSI_MULTI_CPP_RECEIVER_BASE_STATION_H_ |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,21 @@ | ||
#ifndef PIKSI_MULTI_CPP_RECEIVER_POSITION_H_ | ||
#define PIKSI_MULTI_CPP_RECEIVER_POSITION_H_ | ||
|
||
#include <ros/ros.h> | ||
#include <string> | ||
#include "piksi_multi_cpp/device.h" | ||
#include "piksi_multi_cpp/receiver.h" | ||
|
||
namespace piksi_multi_cpp { | ||
|
||
class ReceiverPosition : public Receiver { | ||
public: | ||
ReceiverPosition(const ros::NodeHandle& nh, | ||
const std::shared_ptr<Device>& device); | ||
|
||
private: | ||
}; | ||
|
||
} // namespace piksi_multi_cpp | ||
|
||
#endif // PIKSI_MULTI_CPP_RECEIVER_POSITION_H_ |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,44 @@ | ||
#include "piksi_multi_cpp/callback.h" | ||
|
||
#include <ros/console.h> | ||
|
||
// Forward declaration. | ||
#include <libsbp/system.h> | ||
#include "piksi_multi_cpp/callback_heartbeat.h" | ||
|
||
namespace piksi_multi_cpp { | ||
|
||
Callback::Callback(const ros::NodeHandle& nh, const uint16_t sbp_msg_type, | ||
const std::shared_ptr<sbp_state_t>& state) | ||
: nh_(nh), state_(state) { | ||
sbp_register_callback(state.get(), sbp_msg_type, | ||
&piksi_multi_cpp::CallbackHeartbeat::callback_redirect, | ||
this, &sbp_msg_callback_node_); | ||
} | ||
|
||
void Callback::callback_redirect(uint16_t sender_id, uint8_t len, uint8_t msg[], | ||
void* context) { | ||
if (!context) { | ||
ROS_ERROR("Context not set."); | ||
return; | ||
} | ||
// Cast context to instance. | ||
Callback* instance = static_cast<Callback*>(context); | ||
// Execute instance's read function. | ||
return instance->callback(sender_id, len, msg); | ||
} | ||
|
||
std::shared_ptr<Callback> Callback::create( | ||
const ros::NodeHandle& nh, const uint16_t sbp_msg_type, | ||
const std::shared_ptr<sbp_state_t>& state) { | ||
switch (sbp_msg_type) { | ||
case SBP_MSG_HEARTBEAT: | ||
return std::shared_ptr<Callback>( | ||
new CallbackHeartbeat(nh, SBP_MSG_HEARTBEAT, state)); | ||
default: | ||
ROS_ERROR("Message type %u not implemented.", sbp_msg_type); | ||
return nullptr; | ||
} | ||
} | ||
|
||
} // namespace piksi_multi_cpp |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Hm there's a few problems with this class. Callback is quite a non-ideal name, as it is not immediately clear what it does (breaks principle of least surprise).
Also I think it is a bit overkill to create a inherited Callback type for each different message. I would rather group them into one class, e.g.having a PositionCallbackHandler that contains all callbacks and publishers to output positions (I think there are quite a few of this). Then maybe a DebugCallbackHandler that contains all callbacks with system states, satelite count etc. and so on.
And then depending on the receiver type, it will contain more or less of these grouped handlers. I think that would make it a bit more readable
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I will address this issue in a separate PR as discussed. We will keep the sbp callback handler base class but
a) Try to allow multiple callbacks per message such that individual callback handlers can be defined for the same sbp callback
b) Create inherited superclasses that summarize behavior of similar callbacks, e.g., position callbacks.
c) Template the callback handlers such that they can automatically process
d) Create a RelayCallback class that handles the simple case of relaying a message and has a 1to1 mapping between sbp_msg_type and callback handler.