Commit 8485691b authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[ros] Moved SeatracNode to dedicated file

parent 2a10b479
......@@ -40,6 +40,7 @@ target_link_libraries(seatrac_usbl_node PRIVATE
)
add_executable(seatrac_usbl_pinger
src/SeatracNode.cpp
src/seatrac_usbl_pinger.cpp
)
target_include_directories(seatrac_usbl_pinger PRIVATE src)
......
#include "SeatracNode.h"
namespace narval { namespace seatrac {
using namespace narval::seatrac;
namespace seatrac_msgs = narval::seatrac::messages;
SeatracNode::SeatracNode(const IoServicePtr& ioService,
const std::string& port) :
SeatracDriver(ioService, port),
node_("seatrac_usbl_station"),
beaconId_(BEACON_ALL),
pingCount_(0),
pingSentPublisher_ (node_.advertise<seatrac_usbl::PingSent> ("ping_sent", 100)),
pingErrorPublisher_(node_.advertise<seatrac_usbl::PingError>("ping_error", 100)),
pingResponsePublisher_(
node_.advertise<seatrac_usbl::AcousticFix>("ping_response", 100))
{}
void SeatracNode::on_receive(const std::vector<uint8_t>& data)
{
this->SeatracDriver::on_receive(data);
CID_E msgId = (CID_E)data[0];
switch(msgId) {
default: break;
case CID_PING_ERROR:
{
seatrac_msgs::PingError response;
response = data;
seatrac_usbl::PingError msg;
msg.pingId = pingCount_ - 1;
msg.senderId = beaconId_;
msg.targetId = response.beaconId;
msg.errorCode = response.statusCode;
msg.errorStr = print_utils::to_string(response.statusCode);
pingErrorPublisher_.publish(msg);
std::cout << response << std::endl;
}
break;
case CID_PING_RESP:
{
seatrac_msgs::PingResp response;
response = data;
seatrac_usbl::AcousticFix msg;
copy_to_ros(msg, response.acoFix);
pingResponsePublisher_.publish(msg);
}
break;
}
}
bool SeatracNode::ping(uint8_t targetId)
{
if(targetId > 15) {
std::ostringstream oss;
oss << "Invalid beacon id : " << (uint32_t)targetId
<< ". (Beacon id should be either 0 for broadcast or between 1 and 15)";
throw std::runtime_error(oss.str());
}
if(command::xcvr_status(*this).statusCode != CST_XCVR_STATE_IDLE) {
// acoustic channel not idle
return false;
}
auto result = command::ping_send(*this, (BID_E)targetId, MSG_REQX);
if(result.statusCode != CST_OK) {
// std::cout << "Got a Ping Response" << std::endl << std::flush;
std::cerr << "Ping not sent :\n" << result << std::endl;
return false;
}
seatrac_usbl::PingSent msg;
msg.pingId = pingCount_;
msg.senderId = beaconId_;
msg.targetId = targetId;
pingSentPublisher_.publish(msg);
// Publish PingSent message here
pingCount_++;
return true;
}
}; //namespace seatrac
}; //namespace narval
#ifndef _DEF_SETRAC_USBL_ROS_SEATRAC_NODE_H_
#define _DEF_SETRAC_USBL_ROS_SEATRAC_NODE_H_
#include <iostream>
#include <sstream>
#include <ros/ros.h>
#include <seatrac_usbl/PingSent.h>
#include <seatrac_usbl/PingError.h>
#include <seatrac_driver/SeatracDriver.h>
#include <seatrac_driver/commands.h>
#include <seatrac_driver/messages/Messages.h>
#include "conversions.h"
namespace narval { namespace seatrac {
class SeatracNode : public SeatracDriver
{
public:
using IoService = boost::asio::io_service;
using IoServicePtr = std::shared_ptr<boost::asio::io_service>;
using SerialPort = boost::asio::serial_port;
using ReadBuffer = boost::asio::streambuf;
protected:
ros::NodeHandle node_;
BID_E beaconId_;
uint32_t pingCount_;
ros::Publisher pingSentPublisher_;
ros::Publisher pingErrorPublisher_;
ros::Publisher pingResponsePublisher_;
virtual void on_receive(const std::vector<uint8_t>& data);
public:
SeatracNode(const IoServicePtr& ioService,
const std::string& port = "/dev/narval_usbl");
bool ping(uint8_t targetId);
};
}; //namespace seatrac
}; //namespace narval
#endif //_DEF_SETRAC_USBL_ROS_SEATRAC_NODE_H_
......@@ -14,104 +14,7 @@ using namespace narval::seatrac;
namespace seatrac_msgs = narval::seatrac::messages;
#include "conversions.h"
class SeatracNode : public SeatracDriver
{
public:
using IoService = boost::asio::io_service;
using IoServicePtr = std::shared_ptr<boost::asio::io_service>;
using SerialPort = boost::asio::serial_port;
using ReadBuffer = boost::asio::streambuf;
protected:
ros::NodeHandle node_;
BID_E beaconId_;
uint32_t pingCount_;
ros::Publisher pingSentPublisher_;
ros::Publisher pingErrorPublisher_;
ros::Publisher pingResponsePublisher_;
virtual void on_receive(const std::vector<uint8_t>& data)
{
this->SeatracDriver::on_receive(data);
CID_E msgId = (CID_E)data[0];
switch(msgId) {
default: break;
case CID_PING_ERROR:
{
seatrac_msgs::PingError response;
response = data;
seatrac_usbl::PingError msg;
msg.pingId = pingCount_ - 1;
msg.senderId = beaconId_;
msg.targetId = response.beaconId;
msg.errorCode = response.statusCode;
msg.errorStr = print_utils::to_string(response.statusCode);
pingErrorPublisher_.publish(msg);
std::cout << response << std::endl;
}
break;
case CID_PING_RESP:
{
seatrac_msgs::PingResp response;
response = data;
seatrac_usbl::AcousticFix msg;
copy_to_ros(msg, response.acoFix);
pingResponsePublisher_.publish(msg);
}
break;
}
}
public:
SeatracNode(const IoServicePtr& ioService,
const std::string& port = "/dev/narval_usbl") :
SeatracDriver(ioService, port),
node_("seatrac_usbl_station"),
beaconId_(BEACON_ALL),
pingCount_(0),
pingSentPublisher_ (node_.advertise<seatrac_usbl::PingSent> ("ping_sent", 100)),
pingErrorPublisher_(node_.advertise<seatrac_usbl::PingError>("ping_error", 100)),
pingResponsePublisher_(
node_.advertise<seatrac_usbl::AcousticFix>("ping_response", 100))
{}
bool ping(uint8_t targetId)
{
if(targetId > 15) {
std::ostringstream oss;
oss << "Invalid beacon id : " << (uint32_t)targetId
<< ". (Beacon id should be either 0 for broadcast or between 1 and 15)";
throw std::runtime_error(oss.str());
}
if(command::xcvr_status(*this).statusCode != CST_XCVR_STATE_IDLE) {
// acoustic channel not idle
return false;
}
auto result = command::ping_send(*this, (BID_E)targetId, MSG_REQX);
if(result.statusCode != CST_OK) {
// std::cout << "Got a Ping Response" << std::endl << std::flush;
std::cerr << "Ping not sent :\n" << result << std::endl;
return false;
}
seatrac_usbl::PingSent msg;
msg.pingId = pingCount_;
msg.senderId = beaconId_;
msg.targetId = targetId;
pingSentPublisher_.publish(msg);
// Publish PingSent message here
pingCount_++;
return true;
}
};
#include "SeatracNode.h"
int main(int argc, char** argv)
{
......
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