Commit 48c973e6 authored by Pierre NARVOR's avatar Pierre NARVOR
Browse files

[ros] Added base of a ros1 node

parent e2e88037
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
)
<?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>
#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;
}
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