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}) diff --git a/src/an_driver.cpp b/src/an_driver.cpp index fb83f53..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 // @@ -59,6 +63,8 @@ int main(int argc, char *argv[]) { std::string imu_frame_id; 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]); @@ -72,6 +78,8 @@ 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); + pnh.param("remove_gravity", remove_gravity, false); // Initialise Publishers and Topics // ros::Publisher nav_sat_fix_pub=nh.advertise(topic_prefix + "/NavSatFix",10); @@ -134,6 +142,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)) @@ -143,6 +152,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()) @@ -161,6 +172,13 @@ 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) + { + 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; @@ -205,26 +223,25 @@ 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; - - 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]; + 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) + { + 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 = ""; @@ -404,10 +421,34 @@ int main(int argc, char *argv[]) { imu_msg.orientation_covariance[8] = quaternion_orientation_standard_deviation_packet.standard_deviation[2]; } } + + // 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]; + + } + } + // Ensure that you free the an_packet when your done with it // // 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) {