Commit 38cf0cb8 authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[ros] Updated crude ros node to publish ping data as pose

parent f9e84c98
......@@ -10,6 +10,7 @@ using namespace Eigen;
#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;
......@@ -20,7 +21,26 @@ void print(const uint8_t* data)
cout << *reinterpret_cast<const T*>(data) << endl;
}
class SeatracNode : public SeatracSerial
geometry_msgs::PoseWithCovarianceStamped empty_pose()
{
geometry_msgs::PoseWithCovarianceStamped pose;
pose.header.frame_id = "map";
for(int i = 0; i < 36; i++) {
pose.pose.covariance[i] = 0.0;
}
pose.pose.pose.position.x = 0.0;
pose.pose.pose.position.y = 0.0;
pose.pose.pose.position.z = 0.0;
pose.pose.pose.orientation.w = 1.0;
pose.pose.pose.orientation.x = 0.0;
pose.pose.pose.orientation.y = 0.0;
pose.pose.pose.orientation.z = 0.0;
return pose;
}
class SeatracNode : public SeatracDriver
{
public:
......@@ -36,74 +56,65 @@ class SeatracNode : public SeatracSerial
virtual void on_receive(const std::vector<uint8_t>& data)
{
std::cout << "Received : " << data.size() << " bytes." << std::endl;
seatrac_msgs::Status header = *reinterpret_cast<const seatrac_msgs::Status*>(data.data());
if(header.msgId != CID_STATUS) {
return;
}
if(header.expected_size() != data.size()) {
std::cerr << "Got status but wrong number of bytes (expected "
<< header.expected_size() << ", got " << data.size() << ")\n";
return;
}
const uint8_t* p = data.data() + sizeof(seatrac_msgs::Status);
if(header.contentType & ENVIRONMENT) {
//print<seatrac_msgs::StatusEnvironment>(p);
p += sizeof(seatrac_msgs::StatusEnvironment);
}
if(header.contentType & ATTITUDE) {
print<seatrac_msgs::StatusAttitude>(p);
this->publish_pose(*reinterpret_cast<const seatrac_msgs::StatusAttitude*>(p));
p += sizeof(seatrac_msgs::StatusAttitude);
}
if(header.contentType & MAG_CAL) {
//print<seatrac_msgs::StatusMagCalibration>(p);
p += sizeof(seatrac_msgs::StatusMagCalibration);
}
if(header.contentType & ACC_CAL) {
//print<seatrac_msgs::StatusAccCalibration>(p);
p += sizeof(seatrac_msgs::StatusAccCalibration);
}
if(header.contentType & AHRS_RAW_DATA) {
//print<seatrac_msgs::StatusRawAHRS>(p);
p += sizeof(seatrac_msgs::StatusRawAHRS);
}
if(header.contentType & AHRS_COMP_DATA) {
//print<seatrac_msgs::StatusCompensatedAHRS>(p);
p += sizeof(seatrac_msgs::StatusCompensatedAHRS);
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;
std::cout << response << std::endl;
}
break;
case CID_PING_RESP:
// std::cout << "Got a Ping Response" << std::endl << std::flush;
{
seatrac_msgs::PingResp response;
response = data;
std::cout << response << std::endl;
this->publish_pose(response);
}
break;
}
}
void publish_pose(const seatrac_msgs::StatusAttitude& att)
void publish_pose(const seatrac_msgs::PingResp& response)
{
Quaternionf q;
q = AngleAxisf(0.1f*M_PI/180.0 * att.attYaw, Vector3f::UnitZ())
* AngleAxisf(0.1f*M_PI/180.0 * att.attPitch, Vector3f::UnitY())
* AngleAxisf(0.1f*M_PI/180.0 * att.attRoll, Vector3f::UnitX());
geometry_msgs::PoseWithCovarianceStamped pose;
pose.header.frame_id = "map";
for(int i = 0; i < 36; i++) {
pose.pose.covariance[i] = 0.0;
// Quaternionf q;
// q = AngleAxisf(0.1f*M_PI/180.0 * att.attYaw, Vector3f::UnitZ())
// * AngleAxisf(0.1f*M_PI/180.0 * att.attPitch, Vector3f::UnitY())
// * AngleAxisf(0.1f*M_PI/180.0 * att.attRoll, Vector3f::UnitX());
// geometry_msgs::PoseWithCovarianceStamped pose;
// pose.header.frame_id = "map";
// for(int i = 0; i < 36; i++) {
// pose.pose.covariance[i] = 0.0;
// }
// pose.pose.pose.position.x = 0.0;
// pose.pose.pose.position.y = 0.0;
// pose.pose.pose.position.z = 0.0;
// pose.pose.pose.orientation.w = q.w();
// pose.pose.pose.orientation.x = q.x();
// pose.pose.pose.orientation.y = q.y();
// pose.pose.pose.orientation.z = q.z();
if(response.acoFix.flags & 0x4) {
auto pose = empty_pose();
pose.pose.pose.position.x = 0.1 * response.acoFix.position.easting;
pose.pose.pose.position.y = 0.1 * response.acoFix.position.northing;
pose.pose.pose.position.z = 0.1 * response.acoFix.position.depth;
publisher_.publish(pose);
}
pose.pose.pose.position.x = 0.0;
pose.pose.pose.position.y = 0.0;
pose.pose.pose.position.z = 0.0;
pose.pose.pose.orientation.w = q.w();
pose.pose.pose.orientation.x = q.x();
pose.pose.pose.orientation.y = q.y();
pose.pose.pose.orientation.z = q.z();
publisher_.publish(pose);
}
public:
SeatracNode(const IoServicePtr& ioService,
const std::string& port = "/dev/narval_usbl") :
SeatracSerial(ioService, port),
SeatracDriver(ioService, port),
node_("seatrac_usbl_station"),
publisher_(node_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose", 100))
{}
......@@ -114,56 +125,25 @@ int main(int argc, char** argv)
ros::init(argc, argv, "seatrac_usbl_station");
AsyncService service;
SeatracNode serial(service.io_service(), "/dev/ttyUSB0");
SeatracNode seatrac(service.io_service(), "/dev/ttyUSB0");
service.start();
ros::spin();
service.stop();
// ros::Publisher publisher = node.advertise
// <geometry_msgs::PoseWithCovarianceStamped>("pose", 100);
// Eigen::Quaternionf dq;
// dq.w() = cos(0.1);
// dq.x() = 0.0;
// dq.y() = 0.0;
// dq.z() = sin(0.1);
// Eigen::Quaternionf q0;
// q0.w() = 1.0;
// q0.x() = 0.0;
// q0.y() = 0.0;
// q0.z() = 0.0;
// geometry_msgs::PoseWithCovarianceStamped pose;
// pose.header.frame_id = "map";
// for(int i = 0; i < 36; i++) {
// pose.pose.covariance[i] = 0.0;
// }
// pose.pose.pose.position.x = 0.0;
// pose.pose.pose.position.y = 0.0;
// pose.pose.pose.position.z = 0.0;
// pose.pose.pose.orientation.w = q0.w();
// pose.pose.pose.orientation.x = q0.x();
// pose.pose.pose.orientation.y = q0.y();
// pose.pose.pose.orientation.z = q0.z();
// ros::Rate loopRate(10);
// while(ros::ok()) {
// q0 = q0 * dq;
// pose.pose.pose.orientation.w = q0.w();
// pose.pose.pose.orientation.x = q0.x();
// pose.pose.pose.orientation.y = q0.y();
// pose.pose.pose.orientation.z = q0.z();
// publisher.publish(pose);
// ros::spinOnce();
// loopRate.sleep();
// }
//ros::spin();
ros::Rate loopRate(10);
while(ros::ok()) {
try {
if(command::xcvr_status(seatrac).statusCode == CST_XCVR_STATE_IDLE) {
cout << command::ping_send(seatrac, BEACON_ID_15, MSG_REQU) << endl;
}
}
catch(const TimeoutReached&) {
std::cerr << "Timeout while waiting for request response" << std::endl;
}
ros::spinOnce();
loopRate.sleep();
}
service.stop();
return 0;
}
......
......@@ -31,6 +31,7 @@ void SeatracDriver::on_receive(const std::vector<uint8_t>& data)
}
}
}
return;
switch(msgId) {
default:
std::cout << "Got message : " << msgId << std::endl << std::flush;
......
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