Commit 2a10b479 authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[ros] Added a pinger node (continously ping remote beacon)

parent 86daa029
......@@ -39,6 +39,16 @@ target_link_libraries(seatrac_usbl_node PRIVATE
Eigen3::Eigen
)
add_executable(seatrac_usbl_pinger
src/seatrac_usbl_pinger.cpp
)
target_include_directories(seatrac_usbl_pinger PRIVATE src)
target_link_libraries(seatrac_usbl_pinger PRIVATE
${catkin_LIBRARIES}
seatrac_driver
)
add_dependencies(seatrac_usbl_pinger
seatrac_usbl_generate_messages_cpp
)
uint32 pingId
uint8 senderId
uint8 remoteId
uint8 targetId
uint8 errorCode
string errorStr
uint32 pingId
uint8 senderId
uint8 remoteId
uint8 targetId
#include <iostream>
#include <sstream>
using namespace std;
#include <ros/ros.h>
#include <seatrac_usbl/PingSent.h>
#include <seatrac_usbl/PingError.h>
#include <seatrac_driver/AsyncService.h>
#include <seatrac_driver/SeatracSerial.h>
#include <seatrac_driver/commands.h>
#include <seatrac_driver/messages/Messages.h>
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;
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "seatrac_usbl_station");
AsyncService service;
SeatracNode seatrac(service.io_service(), "/dev/ttyUSB0");
service.start();
ros::Rate loopRate(10);
while(ros::ok()) {
try {
seatrac.ping(15);
}
catch(const TimeoutReached&) {
std::cerr << "Timeout reached while waiting for seatrac feedback." << std::endl;
}
ros::spinOnce();
loopRate.sleep();
}
service.stop();
return 0;
}
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