Skip to content
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

Merged
merged 19 commits into from
Oct 10, 2019
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 6 additions & 1 deletion piksi_multi_cpp/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,12 @@ catkin_simple(ALL_DEPS_REQUIRED)
cs_add_library(${PROJECT_NAME}
src/device.cc
src/device_usb.cc
src/piksi_multi.cc
src/receiver_attitude.cc
src/receiver_base_station.cc
src/receiver_position.cc
src/receiver.cc
src/callback.cc
src/callback_heartbeat.cc
)
target_link_libraries(${PROJECT_NAME} serialport)

Expand Down
50 changes: 50 additions & 0 deletions piksi_multi_cpp/include/piksi_multi_cpp/callback.h
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 {
Copy link
Member

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

Copy link
Collaborator Author

@rikba rikba Oct 9, 2019

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.

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_
21 changes: 21 additions & 0 deletions piksi_multi_cpp/include/piksi_multi_cpp/callback_heartbeat.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_
4 changes: 2 additions & 2 deletions piksi_multi_cpp/include/piksi_multi_cpp/device.h
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,6 @@
#include <string>
#include <vector>

enum DeviceType { kUSB = 0 };

typedef std::string Identifier;
typedef std::set<Identifier> Identifiers;
inline bool identifierEqual(const Identifier& a, const Identifier& b) {
Expand All @@ -18,6 +16,8 @@ inline bool identifierEqual(const Identifier& a, const Identifier& b) {
namespace piksi_multi_cpp {
class Device {
public:
enum DeviceType { kUSB = 0 };

Device(const Identifier& id);
virtual bool open() = 0;
virtual int32_t read(uint8_t* buff, uint32_t n) const = 0;
Expand Down
53 changes: 0 additions & 53 deletions piksi_multi_cpp/include/piksi_multi_cpp/piksi_multi.h

This file was deleted.

86 changes: 86 additions & 0 deletions piksi_multi_cpp/include/piksi_multi_cpp/receiver.h
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,
Copy link
Member

Choose a reason for hiding this comment

The 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_
23 changes: 23 additions & 0 deletions piksi_multi_cpp/include/piksi_multi_cpp/receiver_attitude.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_
21 changes: 21 additions & 0 deletions piksi_multi_cpp/include/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_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_
21 changes: 21 additions & 0 deletions piksi_multi_cpp/include/piksi_multi_cpp/receiver_position.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_
44 changes: 44 additions & 0 deletions piksi_multi_cpp/src/callback.cc
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
Loading