From f6eea0f83478bdbdd7174a7f01d7c3b28c75c604 Mon Sep 17 00:00:00 2001 From: cosama Date: Thu, 7 Jun 2018 11:36:39 -0700 Subject: [PATCH 1/5] Fixed CmakeLists.txt so that it can be build in isolated workspace, removed flag std=c++11 as it creates a warning --- CMakeLists.txt | 23 +++++++++-------------- 1 file changed, 9 insertions(+), 14 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 91cc74b..9b11515 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3) project(advanced_navigation_driver) ## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++11) +#add_compile_options(-std=c++11) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) @@ -132,7 +132,7 @@ include_directories( ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/an_driver_node.cpp) +add_executable(advanced_navigation_driver src/an_driver.cpp src/spatial_packets.c src/an_packet_protocol.c src/rs232/rs232.c) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -145,9 +145,10 @@ include_directories( # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) +target_link_libraries(advanced_navigation_driver + ${catkin_LIBRARIES} +) + ############# ## Install ## @@ -164,11 +165,11 @@ include_directories( # ) ## Mark executables and/or libraries for installation -# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +install(TARGETS ${PROJECT_NAME} advanced_navigation_driver # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) ## Mark cpp header files for installation # install(DIRECTORY include/${PROJECT_NAME}/ @@ -196,9 +197,3 @@ include_directories( ## Add folders to be run by python nosetests # catkin_add_nosetests(test) - -######################### -## Advanced Navigation ## -######################### -add_executable(advanced_navigation_driver src/an_driver.cpp src/spatial_packets.c src/an_packet_protocol.c src/rs232/rs232.c) -target_link_libraries(advanced_navigation_driver ${catkin_LIBRARIES}) From b98fa8811cae20a21d903aeb49ce8b3bdbd8f4f8 Mon Sep 17 00:00:00 2001 From: cosama Date: Thu, 7 Jun 2018 14:54:00 -0700 Subject: [PATCH 2/5] By default the driver publishes the ros time now, behaviour can be changed with ros param device_time --- src/an_driver.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/an_driver.cpp b/src/an_driver.cpp index fb83f53..3fad0d1 100644 --- a/src/an_driver.cpp +++ b/src/an_driver.cpp @@ -59,6 +59,7 @@ int main(int argc, char *argv[]) { std::string imu_frame_id; std::string nav_sat_frame_id; std::string topic_prefix; + bool device_time; if (argc >= 3) { com_port = std::string(argv[1]); @@ -72,6 +73,7 @@ int main(int argc, char *argv[]) { pnh.param("imu_frame_id", imu_frame_id, std::string("imu")); pnh.param("nav_sat_frame_id", nav_sat_frame_id, std::string("gps")); pnh.param("topic_prefix", topic_prefix, std::string("an_device")); + pnh.param("device_time", device_time, false); // Initialise Publishers and Topics // ros::Publisher nav_sat_fix_pub=nh.advertise(topic_prefix + "/NavSatFix",10); @@ -161,6 +163,13 @@ int main(int argc, char *argv[]) { { if(decode_system_state_packet(&system_state_packet, an_packet) == 0) { + if(!device_time) + { + ros::Time ros_time = ros::Time::now(); + system_state_packet.unix_time_seconds = ros_time.sec; + system_state_packet.microseconds = ros_time.nsec/1000; + } + // NavSatFix nav_sat_fix_msg.header.stamp.sec=system_state_packet.unix_time_seconds; nav_sat_fix_msg.header.stamp.nsec=system_state_packet.microseconds*1000; From 1f86041e4c53ac6abd829670062deca06e3f0df8 Mon Sep 17 00:00:00 2001 From: cosama Date: Thu, 7 Jun 2018 16:52:22 -0700 Subject: [PATCH 3/5] Added a raw_sensor_message to get direct access at the unadjusted acceleration data (no gravity subtraction) --- src/an_driver.cpp | 22 +++++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/src/an_driver.cpp b/src/an_driver.cpp index 3fad0d1..94d6b07 100644 --- a/src/an_driver.cpp +++ b/src/an_driver.cpp @@ -136,6 +136,7 @@ int main(int argc, char *argv[]) { an_packet_t *an_packet; system_state_packet_t system_state_packet; quaternion_orientation_standard_deviation_packet_t quaternion_orientation_standard_deviation_packet; + raw_sensors_packet_t raw_sensors_packet; int bytes_received; if (OpenComport(const_cast(com_port.c_str()), baud_rate)) @@ -231,9 +232,9 @@ int main(int argc, char *argv[]) { imu_msg.angular_velocity.x=system_state_packet.angular_velocity[0]; // These the same as the TWIST msg values imu_msg.angular_velocity.y=system_state_packet.angular_velocity[1]; imu_msg.angular_velocity.z=system_state_packet.angular_velocity[2]; - imu_msg.linear_acceleration.x=system_state_packet.body_acceleration[0]; - imu_msg.linear_acceleration.y=system_state_packet.body_acceleration[1]; - imu_msg.linear_acceleration.z=system_state_packet.body_acceleration[2]; + //imu_msg.linear_acceleration.x=system_state_packet.body_acceleration[0]; + //imu_msg.linear_acceleration.y=system_state_packet.body_acceleration[1]; + //imu_msg.linear_acceleration.z=system_state_packet.body_acceleration[2]; // System Status system_status_msg.message = ""; @@ -413,6 +414,21 @@ int main(int argc, char *argv[]) { imu_msg.orientation_covariance[8] = quaternion_orientation_standard_deviation_packet.standard_deviation[2]; } } + + // fix imu acceleration // + if (an_packet->id == packet_id_raw_sensors) + { + // copy all the binary data into the typedef struct for the packet // + // this allows easy access to all the different values // + if(decode_raw_sensors_packet(&raw_sensors_packet, an_packet) == 0) + { + // IMU + imu_msg.linear_acceleration.x = raw_sensors_packet.accelerometers[0]; + imu_msg.linear_acceleration.y = raw_sensors_packet.accelerometers[1]; + imu_msg.linear_acceleration.z = raw_sensors_packet.accelerometers[2]; + } + } + // Ensure that you free the an_packet when your done with it // // or you will leak memory // an_packet_free(&an_packet); From 236ea0d2f72414d664c846233718ebdc70c67a50 Mon Sep 17 00:00:00 2001 From: cosama Date: Fri, 8 Jun 2018 09:25:03 -0700 Subject: [PATCH 4/5] Apply fix (from https://github.com/labust/advanced_navigation_driver) to block on linux reducing CPU usage drastically --- src/an_driver.cpp | 35 +++++++++++++++++++++++++---------- src/rs232/rs232.c | 4 ++-- 2 files changed, 27 insertions(+), 12 deletions(-) diff --git a/src/an_driver.cpp b/src/an_driver.cpp index 94d6b07..342d6c6 100644 --- a/src/an_driver.cpp +++ b/src/an_driver.cpp @@ -59,7 +59,7 @@ int main(int argc, char *argv[]) { std::string imu_frame_id; std::string nav_sat_frame_id; std::string topic_prefix; - bool device_time; + bool device_time, remove_gravity; if (argc >= 3) { com_port = std::string(argv[1]); @@ -74,6 +74,7 @@ int main(int argc, char *argv[]) { pnh.param("nav_sat_frame_id", nav_sat_frame_id, std::string("gps")); pnh.param("topic_prefix", topic_prefix, std::string("an_device")); pnh.param("device_time", device_time, false); + pnh.param("remove_gravity", remove_gravity, false); // Initialise Publishers and Topics // ros::Publisher nav_sat_fix_pub=nh.advertise(topic_prefix + "/NavSatFix",10); @@ -146,6 +147,8 @@ int main(int argc, char *argv[]) { } an_decoder_initialise(&an_decoder); + long long ros_last = ros::Time::now().toNSec()/1000; + ros::Time ros_time = ros::Time::now(); // Loop continuously, polling for packets while (ros::ok()) @@ -164,9 +167,9 @@ int main(int argc, char *argv[]) { { if(decode_system_state_packet(&system_state_packet, an_packet) == 0) { + ros_time = ros::Time::now(); if(!device_time) { - ros::Time ros_time = ros::Time::now(); system_state_packet.unix_time_seconds = ros_time.sec; system_state_packet.microseconds = ros_time.nsec/1000; } @@ -229,12 +232,15 @@ int main(int argc, char *argv[]) { imu_msg.orientation.z=cos_phi * cos_theta * sin_psi - sin_phi * sin_theta * cos_psi; imu_msg.orientation.w=cos_phi * cos_theta * cos_psi + sin_phi * sin_theta * sin_psi; - imu_msg.angular_velocity.x=system_state_packet.angular_velocity[0]; // These the same as the TWIST msg values - imu_msg.angular_velocity.y=system_state_packet.angular_velocity[1]; - imu_msg.angular_velocity.z=system_state_packet.angular_velocity[2]; - //imu_msg.linear_acceleration.x=system_state_packet.body_acceleration[0]; - //imu_msg.linear_acceleration.y=system_state_packet.body_acceleration[1]; - //imu_msg.linear_acceleration.z=system_state_packet.body_acceleration[2]; + if(remove_gravity) + { + imu_msg.angular_velocity.x=system_state_packet.angular_velocity[0]; // These the same as the TWIST msg values + imu_msg.angular_velocity.y=system_state_packet.angular_velocity[1]; + imu_msg.angular_velocity.z=system_state_packet.angular_velocity[2]; + imu_msg.linear_acceleration.x=system_state_packet.body_acceleration[0]; + imu_msg.linear_acceleration.y=system_state_packet.body_acceleration[1]; + imu_msg.linear_acceleration.z=system_state_packet.body_acceleration[2]; + } // System Status system_status_msg.message = ""; @@ -415,17 +421,21 @@ int main(int argc, char *argv[]) { } } - // fix imu acceleration // - if (an_packet->id == packet_id_raw_sensors) + // raw imu data // + if (!remove_gravity && an_packet->id == packet_id_raw_sensors) { // copy all the binary data into the typedef struct for the packet // // this allows easy access to all the different values // if(decode_raw_sensors_packet(&raw_sensors_packet, an_packet) == 0) { // IMU + imu_msg.angular_velocity.x = raw_sensors_packet.gyroscopes[0]; + imu_msg.angular_velocity.y = raw_sensors_packet.gyroscopes[1]; + imu_msg.angular_velocity.z = raw_sensors_packet.gyroscopes[2]; imu_msg.linear_acceleration.x = raw_sensors_packet.accelerometers[0]; imu_msg.linear_acceleration.y = raw_sensors_packet.accelerometers[1]; imu_msg.linear_acceleration.z = raw_sensors_packet.accelerometers[2]; + } } @@ -433,6 +443,11 @@ int main(int argc, char *argv[]) { // or you will leak memory // an_packet_free(&an_packet); + // Make sure packages are only published if the time stamp actually differs // + long long ros_msec = ros_time.toNSec()/1000; + if(ros_msec <= ros_last) continue; + ros_last = ros_msec; + // Publish messages // nav_sat_fix_pub.publish(nav_sat_fix_msg); twist_pub.publish(twist_msg); diff --git a/src/rs232/rs232.c b/src/rs232/rs232.c index 2b30f66..7da04cf 100644 --- a/src/rs232/rs232.c +++ b/src/rs232/rs232.c @@ -101,7 +101,7 @@ int OpenComport(char *comport, int baudrate) break; } - Cport = open(comport, O_RDWR | O_NOCTTY | O_NDELAY); + Cport = open(comport, O_RDWR | O_NOCTTY); //non blocking by removing '| O_NDELAY' if(Cport==-1) { perror("unable to open comport "); @@ -122,7 +122,7 @@ int OpenComport(char *comport, int baudrate) new_port_settings.c_oflag = 0; new_port_settings.c_lflag = 0; new_port_settings.c_cc[VMIN] = 0; /* block untill n bytes are received */ - new_port_settings.c_cc[VTIME] = 0; /* block untill a timer expires (n * 100 mSec.) */ + new_port_settings.c_cc[VTIME] = 1; /* block untill a timer expires (n * 100 mSec.) */ error = tcsetattr(Cport, TCSANOW, &new_port_settings); if(error==-1) { From f594142841a1e1d8d889a6fa51a98908e956d6bb Mon Sep 17 00:00:00 2001 From: cosama Date: Wed, 13 Jun 2018 15:21:23 -0700 Subject: [PATCH 5/5] Changed IMU quaternion to agree with REP 103 as in https://github.com/ros-drivers/advanced_navigation_driver/issues/3#issuecomment-372348146 --- src/an_driver.cpp | 27 ++++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/src/an_driver.cpp b/src/an_driver.cpp index 342d6c6..d12c24d 100644 --- a/src/an_driver.cpp +++ b/src/an_driver.cpp @@ -27,6 +27,8 @@ */ #include +#include + #include #include #include @@ -44,6 +46,8 @@ #include #define RADIANS_TO_DEGREES (180.0/M_PI) +const double PI = 4*atan(1); + int main(int argc, char *argv[]) { // Set up ROS node // @@ -60,6 +64,7 @@ int main(int argc, char *argv[]) { std::string nav_sat_frame_id; std::string topic_prefix; bool device_time, remove_gravity; + tf::Quaternion orientation; if (argc >= 3) { com_port = std::string(argv[1]); @@ -218,19 +223,15 @@ int main(int argc, char *argv[]) { imu_msg.header.stamp.nsec=system_state_packet.microseconds*1000; imu_msg.header.frame_id=imu_frame_id; // Convert roll, pitch, yaw from radians to quaternion format // - float phi = system_state_packet.orientation[0] / 2.0f; - float theta = system_state_packet.orientation[1] / 2.0f; - float psi = system_state_packet.orientation[2] / 2.0f; - float sin_phi = sinf(phi); - float cos_phi = cosf(phi); - float sin_theta = sinf(theta); - float cos_theta = cosf(theta); - float sin_psi = sinf(psi); - float cos_psi = cosf(psi); - imu_msg.orientation.x=-cos_phi * sin_theta * sin_psi + sin_phi * cos_theta * cos_psi; - imu_msg.orientation.y=cos_phi * sin_theta * cos_psi + sin_phi * cos_theta * sin_psi; - imu_msg.orientation.z=cos_phi * cos_theta * sin_psi - sin_phi * sin_theta * cos_psi; - imu_msg.orientation.w=cos_phi * cos_theta * cos_psi + sin_phi * sin_theta * sin_psi; + orientation.setRPY( + system_state_packet.orientation[0], + system_state_packet.orientation[1], + PI / 2.0f - system_state_packet.orientation[2] // REP 103 + ); + imu_msg.orientation.x = orientation[0]; + imu_msg.orientation.y = orientation[1]; + imu_msg.orientation.z = orientation[2]; + imu_msg.orientation.w = orientation[3]; if(remove_gravity) {