Commit e606369c authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[ros] Added a service to change beacon id

parent 6f183990
......@@ -139,6 +139,28 @@ inline messages::SettingsReset settings_reset(SeatracDriver& seatrac, int64_t ti
return response;
}
inline bool set_beacon_id(SeatracDriver& seatrac, BID_E newId,
bool saveSettings = false, int64_t timeout=1000)
{
auto settings = settings_get(seatrac, timeout).settings;
settings.xcvrBeaconId = newId;
auto setOk = settings_set(seatrac, settings, timeout);
if(setOk.statusCode != CST_OK) {
std::cerr << "Error while changing settings : " << setOk << std::endl;
return false;
}
if(saveSettings) {
auto saveOk = settings_save(seatrac, timeout);
if(saveOk.statusCode != CST_OK) {
std::cerr << "Error while saving settings : " << saveOk << std::endl;
return false;
}
}
return true;
}
inline messages::PingSend ping_send(SeatracDriver& seatrac,
BID_E target,
AMSGTYPE_E pingType = MSG_REQ, // MSG_REQ simple ping without USBL info
......
......@@ -20,6 +20,9 @@ add_message_files(FILES
PingSent.msg
PingError.msg
)
add_service_files(FILES
SetBeaconId.srv
)
generate_messages(DEPENDENCIES
std_msgs
)
......
......@@ -18,7 +18,9 @@ SeatracNode::SeatracNode(const IoServicePtr& ioService,
pingSentPublisher_ (node_->advertise<seatrac_usbl::PingSent>(pingSentTopic, 100)),
pingErrorPublisher_(node_->advertise<seatrac_usbl::PingError>(pingErrorTopic, 100)),
pingResponsePublisher_(
node_->advertise<seatrac_usbl::AcousticFix>(pingResponseTopic, 100))
node_->advertise<seatrac_usbl::AcousticFix>(pingResponseTopic, 100)),
changeIdService_(
node->advertiseService("set_beacon_id", &SeatracNode::set_beacon_id, this))
{}
SeatracNode::Ptr SeatracNode::Create(const IoServicePtr& ioService,
......@@ -54,7 +56,7 @@ void SeatracNode::on_receive(const std::vector<uint8_t>& data)
response = data;
seatrac_usbl::PingError msg;
msg.pingId = pingCount_ - 1;
msg.senderId = beaconId_;
msg.senderId = this->beacon_id();
msg.targetId = response.beaconId;
msg.errorCode = response.statusCode;
msg.errorStr = print_utils::to_string(response.statusCode);
......@@ -96,7 +98,7 @@ bool SeatracNode::ping(uint8_t targetId)
seatrac_usbl::PingSent msg;
msg.pingId = pingCount_;
msg.senderId = beaconId_;
msg.senderId = this->beacon_id();
msg.targetId = targetId;
pingSentPublisher_.publish(msg);
......@@ -105,6 +107,51 @@ bool SeatracNode::ping(uint8_t targetId)
return true;
}
BID_E SeatracNode::beacon_id()
{
if(beaconId_ == BEACON_ALL) {
beaconId_ = command::settings_get(*this).settings.xcvrBeaconId;
}
return beaconId_;
}
bool SeatracNode::set_beacon_id(BID_E newId, bool save)
{
return command::set_beacon_id(*this, newId);
}
bool SeatracNode::set_beacon_id(seatrac_usbl::SetBeaconId::Request& req,
seatrac_usbl::SetBeaconId::Response& res)
{
std::ostringstream oss;
if(req.beaconId == 0 || req.beaconId > 15) {
oss << "Invalid beaconId (got " << req.beaconId
<< ", expected between [1-15])";
res.message = oss.str();
res.success = false;
return false;
}
try {
if(!this->set_beacon_id((BID_E)req.beaconId, false)) {
res.success = false;
res.message = "Unknown error";
return false;
}
}
catch(const TimeoutReached&) {
oss << "Timeout reached while spoeaking to the beacon. Is it connected ?";
res.message = oss.str();
res.success = false;
return false;
}
beaconId_ = (BID_E)req.beaconId;
oss << "Beacon id changed successfully to " << (int)req.beaconId;
res.success = true;
res.message = oss.str();
return true;
}
}; //namespace seatrac
}; //namespace narval
......@@ -7,6 +7,7 @@
#include <ros/ros.h>
#include <seatrac_usbl/PingSent.h>
#include <seatrac_usbl/PingError.h>
#include <seatrac_usbl/SetBeaconId.h>
#include <seatrac_driver/SeatracDriver.h>
#include <seatrac_driver/commands.h>
......@@ -30,12 +31,13 @@ class SeatracNode : public SeatracDriver
protected:
NodeHandlePtr node_;
NodeHandlePtr node_;
BID_E beaconId_;
uint32_t pingCount_;
ros::Publisher pingSentPublisher_;
ros::Publisher pingErrorPublisher_;
ros::Publisher pingResponsePublisher_;
ros::ServiceServer changeIdService_;
virtual void on_receive(const std::vector<uint8_t>& data);
......@@ -52,6 +54,11 @@ class SeatracNode : public SeatracDriver
const NodeHandlePtr& node);
bool ping(uint8_t targetId);
BID_E beacon_id();
bool set_beacon_id(BID_E newId, bool save = false);
bool set_beacon_id(seatrac_usbl::SetBeaconId::Request& req,
seatrac_usbl::SetBeaconId::Response& res);
};
......
uint8 beaconId # new beacon Id, must be between [1-15]
---
bool success
string message
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment