[ros] Added base of a ros1 node

ros/CMakeLists.txt

+cmake_minimum_required(VERSION 3.10)
+project(seatrac_usbl VERSION 0.1)
+
+find_package(seatrac_driver REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+find_package(catkin REQUIRED COMPONENTS
+    roscpp
+    rospy
+    std_msgs
+    geometry_msgs
+)
+
+catkin_package(CATKIN_DEPENDS roscpp rospy std_msgs geometry_msgs)
+
+# replace with target include directories ?
+include_directories(${catkin_INCLUDE_DIRS})
+
+add_executable(seatrac_usbl_node
+    src/seatrac_usbl_node.cpp
+)
+target_include_directories(seatrac_usbl_node PRIVATE src)
+target_link_libraries(seatrac_usbl_node PRIVATE
+    ${catkin_LIBRARIES}
+    seatrac_driver
+    Eigen3::Eigen
+)
+
+

ros/package.xml

+<?xml version="1.0"?>
+<package format="2">
+  <name>seatrac_usbl</name>
+  <version>0.0.1</version>
+  <description>ROS node for Blueprint Seatrac USBL</description>
+
+  <maintainer email="pierre.narvor@ensta-bretagne.fr">Pierre Narvor</maintainer>
+
+  <license>BSD3</license>
+
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+</package>

ros/src/seatrac_usbl_node.cpp

+#include <iostream>
+using namespace std;
+
+#include <ros/ros.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+
+#include <seatrac_driver/AsyncService.h>
+#include <seatrac_driver/SeatracDriver.h>
+using namespace narval::seatrac;
+
+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");
+
+    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();
+    // }
+
+    return 0;
+}
+