Commit 886e83a8 authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[ros] Simple usbl pose publisher from status messages

parent 48c973e6
#include <iostream>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace Eigen;
#include <ros/ros.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <seatrac_driver/AsyncService.h>
#include <seatrac_driver/SeatracDriver.h>
#include <seatrac_driver/SeatracSerial.h>
#include <seatrac_driver/messages/Messages.h>
using namespace narval::seatrac;
namespace seatrac_msgs = narval::seatrac::messages;
template <typename T>
void print(const uint8_t* data)
{
cout << *reinterpret_cast<const T*>(data) << endl;
}
class SeatracNode : public SeatracSerial
{
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_;
ros::Publisher publisher_;
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.cmdId != 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);
}
}
void publish_pose(const seatrac_msgs::StatusAttitude& att)
{
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();
publisher_.publish(pose);
}
public:
SeatracNode(const IoServicePtr& ioService,
const std::string& port = "/dev/narval_usbl") :
SeatracSerial(ioService, port),
node_("seatrac_usbl"),
publisher_(node_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose", 100))
{}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "seatrac_usbl");
ros::NodeHandle node("seatrac_usbl");
AsyncService service;
SeatracDriver serial(service.io_service(), "/dev/ttyUSB0");
SeatracNode serial(service.io_service(), "/dev/ttyUSB0");
service.start();
ros::spin();
......
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