Skip to content
Snippets Groups Projects
Commit 397f71c9 authored by Maximilien Naveau's avatar Maximilien Naveau Committed by Maximilien Naveau
Browse files

fix remarks from code review

parent 2f32b7f7
No related branches found
No related tags found
No related merge requests found
...@@ -43,7 +43,6 @@ project(${PROJECT_NAME} ${PROJECT_ARGS}) ...@@ -43,7 +43,6 @@ project(${PROJECT_NAME} ${PROJECT_ARGS})
# Project dependencies # Project dependencies
find_package(qualisys_cpp_sdk REQUIRED) find_package(qualisys_cpp_sdk REQUIRED)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC")
find_package(roscpp REQUIRED) find_package(roscpp REQUIRED)
find_package(tf2 REQUIRED) find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED) find_package(tf2_ros REQUIRED)
......
...@@ -24,7 +24,7 @@ bool QualisysToRos::initialize() { ...@@ -24,7 +24,7 @@ bool QualisysToRos::initialize() {
int tmp_base_port(0); int tmp_base_port(0);
int tmp_udp_port(0); int tmp_udp_port(0);
node_handle_->param("server_address", server_address_, node_handle_->param("server_address", server_address_,
std::string("192.168.0.1")); std::string("127.0.0.1"));
node_handle_->param("server_base_port", tmp_base_port, 22222); node_handle_->param("server_base_port", tmp_base_port, 22222);
node_handle_->param("server_udp_port", tmp_udp_port, 6734); node_handle_->param("server_udp_port", tmp_udp_port, 6734);
node_handle_->param("major_version", major_version_, 1); node_handle_->param("major_version", major_version_, 1);
...@@ -126,6 +126,7 @@ void QualisysToRos::run() { ...@@ -126,6 +126,7 @@ void QualisysToRos::run() {
rotation_matrix_[8]); rotation_matrix_[8]);
ros_rotation_matrix_.getRotation(ros_quaternion_); ros_rotation_matrix_.getRotation(ros_quaternion_);
ros_quaternion_.normalize(); ros_quaternion_.normalize();
// There a change of convention between ROS and Qualisys
ros_quaternion_[3] = -ros_quaternion_[3]; ros_quaternion_[3] = -ros_quaternion_[3];
ros_quaternion_.normalize(); ros_quaternion_.normalize();
ros_transform_.transform.translation.x = px_ / 1000.0; ros_transform_.transform.translation.x = px_ / 1000.0;
......
...@@ -7,7 +7,7 @@ ...@@ -7,7 +7,7 @@
#include "ros-qualisys/qualisys-to-ros.hpp" #include "ros-qualisys/qualisys-to-ros.hpp"
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
ros::init(argc, argv, "my_node_name", ros::init_options::NoSigintHandler); ros::init(argc, argv, "ros_qualisys", ros::init_options::NoSigintHandler);
ros::NodeHandle nh; ros::NodeHandle nh;
ros_qualisys::QualisysToRos q2r; ros_qualisys::QualisysToRos q2r;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment