Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

High resource usage #11 #12

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
Open

High resource usage #11 #12

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
f6eea0f
Fixed CmakeLists.txt so that it can be build in isolated workspace, r…
cosama Jun 7, 2018
b98fa88
By default the driver publishes the ros time now, behaviour can be ch…
cosama Jun 7, 2018
1f86041
Added a raw_sensor_message to get direct access at the unadjusted acc…
cosama Jun 7, 2018
236ea0d
Apply fix (from https://github.com/labust/advanced_navigation_driver)…
cosama Jun 8, 2018
f594142
Changed IMU quaternion to agree with REP 103 as in https://github.com…
cosama Jun 13, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 9 additions & 14 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand All @@ -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 ##
Expand All @@ -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}/
Expand Down Expand Up @@ -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})
81 changes: 61 additions & 20 deletions src/an_driver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,8 @@
*/

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
Expand All @@ -44,6 +46,8 @@
#include <diagnostic_msgs/DiagnosticArray.h>

#define RADIANS_TO_DEGREES (180.0/M_PI)
const double PI = 4*atan(1);


int main(int argc, char *argv[]) {
// Set up ROS node //
Expand All @@ -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]);
Expand All @@ -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<sensor_msgs::NavSatFix>(topic_prefix + "/NavSatFix",10);
Expand Down Expand Up @@ -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<char*>(com_port.c_str()), baud_rate))
Expand All @@ -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())
Expand All @@ -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;
Expand Down Expand Up @@ -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 = "";
Expand Down Expand Up @@ -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);
Expand Down
4 changes: 2 additions & 2 deletions src/rs232/rs232.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 ");
Expand All @@ -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)
{
Expand Down