From 52ff48967a031d7a2865be436ccca046d8a2b703 Mon Sep 17 00:00:00 2001 From: Jakob Auer Date: Tue, 4 Sep 2018 14:22:51 +0200 Subject: [PATCH 01/10] adaptions in ros2gams to be able to handle list renaming and ignore members --- src/gams/utility/ros/RosParser.cpp | 91 ++++++++++++++++++++---------- src/gams/utility/ros/RosParser.h | 3 + 2 files changed, 63 insertions(+), 31 deletions(-) diff --git a/src/gams/utility/ros/RosParser.cpp b/src/gams/utility/ros/RosParser.cpp index 8fb7f16dc..fd56bf024 100644 --- a/src/gams/utility/ros/RosParser.cpp +++ b/src/gams/utility/ros/RosParser.cpp @@ -827,38 +827,30 @@ void gams::utility::ros::RosParser::set_dyn_capnp_value( std::size_t dot_pos = var_name.find("."); - try + if (dot_pos != std::string::npos) { - if (dot_pos != std::string::npos) + //This is a list + std::string var = var_name.substr(0, dot_pos); + int index = std::stoi(var_name.substr(dot_pos+1)); + if (index == 0) { - //This is a list - std::string var = var_name.substr(0, dot_pos); - int index = std::stoi(var_name.substr(dot_pos+1)); - if (index == 0) - { - // First element so init - dynvalue.init(var, array_size); - } - auto lst = dynvalue.get(var).as(); - lst.set(index, val); - } - else - { - if (dynvalue.asReader().get(var_name).getType() == capnp::DynamicValue::BOOL) - { - bool bool_val = (bool) val; - dynvalue.set(var_name, bool_val); - } - else - { - dynvalue.set(var_name, val); - } + // First element so init + dynvalue.init(var, array_size); } + auto lst = dynvalue.get(var).as(); + lst.set(index, val); } - catch(kj::Exception ex) + else { - std::cout << "Failed to set " << var_name << " in struct " << - struct_name << "! (" << std::string(ex.getDescription()) << ")" << std::endl; + if (dynvalue.asReader().get(var_name).getType() == capnp::DynamicValue::BOOL) + { + bool bool_val = (bool) val; + dynvalue.set(var_name, bool_val); + } + else + { + dynvalue.set(var_name, val); + } } } @@ -960,7 +952,16 @@ void gams::utility::ros::RosParser::parse_any ( std::string datatype, // Value is NAN if it is not readable } name = substitute_name(datatype, name); - set_dyn_capnp_value(capnp_builder, name, val, array_size); + if (name == IGNORE_MARKER) continue; + try + { + set_dyn_capnp_value(capnp_builder, name, val, array_size); + } + catch(kj::Exception ex) + { + std::cout << "Failed to set " << name << " from topic " << + topic_name << " (" << datatype << ")! (" << std::string(ex.getDescription()) << ")" << std::endl; + } } for (auto it: flat_container.name) @@ -974,7 +975,16 @@ void gams::utility::ros::RosParser::parse_any ( std::string datatype, auto val = strdup(value.c_str()); std::string name = key.substr (topic_len); name = substitute_name(datatype, name); - set_dyn_capnp_value(capnp_builder, name, val, array_size); + if (name == IGNORE_MARKER) continue; + try + { + set_dyn_capnp_value(capnp_builder, name, val, array_size); + } + catch(kj::Exception ex) + { + std::cout << "Failed to set " << name << " from topic " << + topic_name << " (" << datatype << ")! (" << std::string(ex.getDescription()) << ")" << std::endl; + } } ros_array_sizes_.clear(); madara::knowledge::GenericCapnObject any(schema_name.c_str(), buffer); @@ -1057,6 +1067,16 @@ void gams::utility::ros::RosParser::register_rename_rules( std::map values = {type, "general"}; for (std::string it : values) { @@ -1064,10 +1084,19 @@ std::string gams::utility::ros::RosParser::substitute_name(std::string type, if (search != name_substitution_map_.end ()) { std::map name_map = search->second; - auto name_search = name_map.find (name); + + auto name_search = name_map.find (search_name); if (name_search != name_map.end ()) { - return name_search->second; + if (name_search->second == IGNORE_MARKER) + { + return name_search->second; + } + else + { + return name_search->second + array_index; + } + } } } diff --git a/src/gams/utility/ros/RosParser.h b/src/gams/utility/ros/RosParser.h index fb9ea7609..6a86eab8f 100644 --- a/src/gams/utility/ros/RosParser.h +++ b/src/gams/utility/ros/RosParser.h @@ -91,6 +91,9 @@ namespace gams class RosParser { public: + + const std::string IGNORE_MARKER = "_IGNORE_"; + RosParser (knowledge::KnowledgeBase * kb, std::string world_frame, std::string base_frame, std::map capnp_types, From f4df27ac425060a51169d40619118a5f09c533a5 Mon Sep 17 00:00:00 2001 From: Jakob Auer Date: Mon, 10 Sep 2018 21:41:10 +0200 Subject: [PATCH 02/10] fixed bugs caused by changes to ReferenceFrame in ros2gams --- src/gams/utility/ros/RosParser.cpp | 65 +++++++++++++++++------------- src/gams/utility/ros/RosParser.h | 1 + 2 files changed, 37 insertions(+), 29 deletions(-) diff --git a/src/gams/utility/ros/RosParser.cpp b/src/gams/utility/ros/RosParser.cpp index fd56bf024..5dde6a55e 100644 --- a/src/gams/utility/ros/RosParser.cpp +++ b/src/gams/utility/ros/RosParser.cpp @@ -456,10 +456,13 @@ void gams::utility::ros::RosParser::parse_tf_message (tf2_msgs::TFMessage * tf) std::replace ( child_frame_id.begin (), child_frame_id.end (), '/', '_'); // parse the rotation and orientation - gams::pose::ReferenceFrame parent = gams::pose::ReferenceFrame::load ( - *knowledge_, frame_id); - - if (!parent.valid ()) + gams::pose::ReferenceFrame parent; + try + { + parent = gams::pose::ReferenceFrame::load ( + *knowledge_, frame_id); + } + catch (gams::exceptions::ReferenceFrameException) { parent = gams::pose::ReferenceFrame (frame_id, gams::pose::Pose (gams::pose::ReferenceFrame (), 0, 0)); @@ -489,34 +492,38 @@ void gams::utility::ros::RosParser::parse_tf_message (tf2_msgs::TFMessage * tf) { // World and base frames are defined so we can calculate the agent // location and orientation - gams::pose::ReferenceFrame world = gams::pose::ReferenceFrame::load ( - *knowledge_, world_frame_); - gams::pose::ReferenceFrame base = gams::pose::ReferenceFrame::load ( - *knowledge_, base_frame_, max_timestamp); - - if (world.valid () && base.valid ()) + gams::pose::ReferenceFrame world; + gams::pose::ReferenceFrame base; + try { - try - { - gams::pose::Pose base_pose = base.origin ().transform_to (world); - containers::NativeDoubleVector location ("agents.0.location", - *knowledge_, 3, eval_settings_); - containers::NativeDoubleVector orientation ("agents.0.orientation", - *knowledge_, 3, eval_settings_); - location.set (0, base_pose.as_location_vec ().get (0), eval_settings_); - location.set (1, base_pose.as_location_vec ().get (1), eval_settings_); - location.set (2, base_pose.as_location_vec ().get (2), eval_settings_); - orientation.set (0, base_pose.as_orientation_vec ().get (0), - eval_settings_); - orientation.set (1, base_pose.as_orientation_vec ().get (1), - eval_settings_); - orientation.set (2, base_pose.as_orientation_vec ().get (2), - eval_settings_); - - } - catch ( gams::pose::unrelated_frames ex){} + world = gams::pose::ReferenceFrame::load ( + *knowledge_, world_frame_); + base = gams::pose::ReferenceFrame::load ( + *knowledge_, base_frame_, max_timestamp); + } + catch (gams::exceptions::ReferenceFrameException) + { + return; } + try + { + gams::pose::Pose base_pose = base.origin ().transform_to (world); + containers::NativeDoubleVector location ("agents.0.location", + *knowledge_, 3, eval_settings_); + containers::NativeDoubleVector orientation ("agents.0.orientation", + *knowledge_, 3, eval_settings_); + location.set (0, base_pose.as_location_vec ().get (0), eval_settings_); + location.set (1, base_pose.as_location_vec ().get (1), eval_settings_); + location.set (2, base_pose.as_location_vec ().get (2), eval_settings_); + orientation.set (0, base_pose.as_orientation_vec ().get (0), + eval_settings_); + orientation.set (1, base_pose.as_orientation_vec ().get (1), + eval_settings_); + orientation.set (2, base_pose.as_orientation_vec ().get (2), + eval_settings_); + } + catch ( gams::pose::unrelated_frames ex){} } } diff --git a/src/gams/utility/ros/RosParser.h b/src/gams/utility/ros/RosParser.h index 6a86eab8f..ac95c7891 100644 --- a/src/gams/utility/ros/RosParser.h +++ b/src/gams/utility/ros/RosParser.h @@ -22,6 +22,7 @@ #include "gams/pose/ReferenceFrame.h" #include "gams/pose/Pose.h" #include "gams/pose/Quaternion.h" +#include "gams/exceptions/ReferenceFrameException.h" #ifdef __GNUC__ #pragma GCC diagnostic push From 427a4557ba873cc1db1530ccf936c5f53694d7ac Mon Sep 17 00:00:00 2001 From: Jakob Auer Date: Fri, 14 Sep 2018 14:03:51 +0200 Subject: [PATCH 03/10] performance improvements --- src/gams/utility/ros/RosParser.cpp | 44 ++++++++++++++++-------------- src/gams/utility/ros/RosParser.h | 4 +-- using_boost.mpb | 1 + 3 files changed, 27 insertions(+), 22 deletions(-) diff --git a/src/gams/utility/ros/RosParser.cpp b/src/gams/utility/ros/RosParser.cpp index 5dde6a55e..5e74ce54b 100644 --- a/src/gams/utility/ros/RosParser.cpp +++ b/src/gams/utility/ros/RosParser.cpp @@ -865,31 +865,35 @@ void gams::utility::ros::RosParser::set_dyn_capnp_value( /* Determines the size of an ros introspection array */ -template unsigned int gams::utility::ros::RosParser::get_array_size(std::string var_name, - std::vector> array) + RosIntrospection::RenamedValues* array) { + + unsigned int len = 0; std::size_t dot_pos = var_name.find("."); + std::string array_name = var_name.substr(0, dot_pos); if (dot_pos == std::string::npos) { // This is no array - return 1; + len = 1; } - std::string array_name = var_name.substr(0, dot_pos); - - auto cached = ros_array_sizes_.find(array_name); - if (cached != ros_array_sizes_.end()) - { - return cached->second; - } - - unsigned int len = 0; - for ( auto it : array) + else { - std::string key = it.first; - if (key.rfind(array_name, 0) == 0) + auto cached = ros_array_sizes_.find(array_name); + if (cached != ros_array_sizes_.end()) + { + len = cached->second; + } + else { - len++; + for ( auto it : *array) + { + std::string key = it.first; + if (key.rfind(array_name, 0) == 0) + { + len++; + } + } } } ros_array_sizes_[array_name] = len; @@ -923,8 +927,8 @@ void gams::utility::ros::RosParser::parse_any ( std::string datatype, // deserialize and rename the vectors bool success = parser_.deserializeIntoFlatContainer ( topic_name, - absl::Span (parser_buffer), - &flat_container, 2048 ); + absl::Span (parser_buffer), + &flat_container, 10240000 ); if (!success) { @@ -944,7 +948,7 @@ void gams::utility::ros::RosParser::parse_any ( std::string datatype, const std::string& key = it.first; const RosIntrospection::Variant& value = it.second; - int array_size = get_array_size(key, renamed_values); + int array_size = get_array_size(key, &renamed_values); std::string name = key.substr (topic_len); @@ -976,7 +980,7 @@ void gams::utility::ros::RosParser::parse_any ( std::string datatype, const std::string& key = it.first.toStdString (); const std::string& value = it.second; - int array_size = get_array_size(key, renamed_values); + int array_size = get_array_size(key, &renamed_values); auto val = strdup(value.c_str()); diff --git a/src/gams/utility/ros/RosParser.h b/src/gams/utility/ros/RosParser.h index ac95c7891..dcfc212a3 100644 --- a/src/gams/utility/ros/RosParser.h +++ b/src/gams/utility/ros/RosParser.h @@ -250,9 +250,9 @@ namespace gams void set_dyn_capnp_value(capnp::DynamicStruct::Builder builder, std::string name, T val, unsigned int array_size); - template + //template unsigned int get_array_size(std::string var_name, - std::vector> array); + RosIntrospection::RenamedValues* array); /** * Substitutes the names of topic type members based on the registered diff --git a/using_boost.mpb b/using_boost.mpb index 8de5a335d..7b1f8ec1a 100644 --- a/using_boost.mpb +++ b/using_boost.mpb @@ -1,4 +1,5 @@ project { + libpaths += /usr/local/lib expand(BOOST_ROOT_LIB) { $BOOST_ROOT_LIB $(BOOST_ROOT)/stage/lib From 510c47e710c9b2b9c17a8adfbb2e3b2b39b35a1d Mon Sep 17 00:00:00 2001 From: Jakob Auer Date: Wed, 19 Sep 2018 14:57:10 +0200 Subject: [PATCH 04/10] added more comments to rosparser for ros2gams --- src/gams/utility/ros/RosParser.cpp | 100 ++++++++++++++++++++++------- 1 file changed, 77 insertions(+), 23 deletions(-) diff --git a/src/gams/utility/ros/RosParser.cpp b/src/gams/utility/ros/RosParser.cpp index 5e74ce54b..cd131007a 100644 --- a/src/gams/utility/ros/RosParser.cpp +++ b/src/gams/utility/ros/RosParser.cpp @@ -34,13 +34,15 @@ void gams::utility::ros::RosParser::parse_message ( const rosbag::MessageInstance m, std::string container_name) { //Update sim time before parsing - set_sim_time(m.getTime()); + set_sim_time (m.getTime ()); //Parse the message - std::string datatype = m.getDataType(); + std::string datatype = m.getDataType (); auto search = capnp_types_.find(datatype); if (search != capnp_types_.end()) { + // If the message type is in the mapfile we have to parse this message + // into a capnproto schema parse_any(m, container_name); } else if (m.isType ()) @@ -650,7 +652,8 @@ void gams::utility::ros::RosParser::parse_point (geometry_msgs::Point *point_msg * @param quat the geometry_msgs::Quaternion message * @param orientatiom the container **/ -void gams::utility::ros::RosParser::parse_quaternion (geometry_msgs::Quaternion *quat, +void gams::utility::ros::RosParser::parse_quaternion ( + geometry_msgs::Quaternion *quat, containers::NativeDoubleVector *orientation) { tf::Quaternion tfquat (quat->x, quat->y, quat->z, quat->w); @@ -666,7 +669,8 @@ void gams::utility::ros::RosParser::parse_quaternion (geometry_msgs::Quaternion template -void gams::utility::ros::RosParser::parse_float64_array (boost::array *array, +void gams::utility::ros::RosParser::parse_float64_array ( + boost::array *array, containers::NativeDoubleVector *target) { int i = 0; @@ -678,7 +682,8 @@ void gams::utility::ros::RosParser::parse_float64_array (boost::array } } -void gams::utility::ros::RosParser::parse_float64_array (std::vector *array, +void gams::utility::ros::RosParser::parse_float64_array ( + std::vector *array, containers::NativeDoubleVector *target) { int i = 0; @@ -704,7 +709,8 @@ void gams::utility::ros::RosParser::parse_int_array (std::vector *array, } template -void gams::utility::ros::RosParser::parse_int_array (boost::array *array, +void gams::utility::ros::RosParser::parse_int_array ( + boost::array *array, containers::NativeIntegerVector *target) { int i = 0; @@ -738,12 +744,16 @@ std::string gams::utility::ros::ros_to_gams_name (std::string ros_topic_name) return topic; } +/** + * Loads a capnp schema from a specified path + **/ void gams::utility::ros::RosParser::load_capn_schema(std::string path) { try{ int fd = open(path.c_str(), 0, O_RDONLY); capnp::StreamFdMessageReader schema_message_reader(fd); - auto schema_reader = schema_message_reader.getRoot(); + auto schema_reader = + schema_message_reader.getRoot(); for (auto schema : schema_reader.getNodes()) { std::string schema_name = cleanCapnpSchemaName (schema.getDisplayName()); @@ -756,6 +766,9 @@ void gams::utility::ros::RosParser::load_capn_schema(std::string path) } } +/** + * Helper method to print all available schemas + **/ void gams::utility::ros::RosParser::print_schemas() { std::cout << "Available schemas: " << std::endl; @@ -776,12 +789,14 @@ capnp::DynamicStruct::Builder gams::utility::ros::RosParser::get_dyn_capnp_struc { return builder; } - std::string n = name.substr(1); - std::istringstream f(n); + // remove the leading slash + std::string n = name.substr (1); + std::istringstream f (n); std::string s; capnp::DynamicStruct::Builder dyn = builder; - while (getline(f, s, '/')) { + // Iterate through the dynamic struct + while (getline (f, s, '/')) { dyn = dyn.get(s).as(); } return dyn; @@ -798,8 +813,7 @@ void gams::utility::ros::RosParser::set_dyn_capnp_value( T val, unsigned int array_size) { - //name.erase(std::remove (name.begin (), name.end (), '_'), name.end()); - //remove _ values from the name string and chang it to camelcase + //remove _ values from the name string and change it to camelcase std::stringstream camelcase; for (unsigned int i = 0; i < name.size(); i++) { @@ -833,7 +847,6 @@ void gams::utility::ros::RosParser::set_dyn_capnp_value( auto dynvalue = get_dyn_capnp_struct(builder, struct_name); std::size_t dot_pos = var_name.find("."); - if (dot_pos != std::string::npos) { //This is a list @@ -849,8 +862,10 @@ void gams::utility::ros::RosParser::set_dyn_capnp_value( } else { - if (dynvalue.asReader().get(var_name).getType() == capnp::DynamicValue::BOOL) + if (dynvalue.asReader().get(var_name).getType() == + capnp::DynamicValue::BOOL) { + // We need to specifically cast bool values before asignment bool bool_val = (bool) val; dynvalue.set(var_name, bool_val); } @@ -868,7 +883,8 @@ void gams::utility::ros::RosParser::set_dyn_capnp_value( unsigned int gams::utility::ros::RosParser::get_array_size(std::string var_name, RosIntrospection::RenamedValues* array) { - + // This method caches list sizes into the private member ros_array_sizes_. + // IMPORTANT: Keep in mind to clear ros_array_sizes_ after each message!! unsigned int len = 0; std::size_t dot_pos = var_name.find("."); std::string array_name = var_name.substr(0, dot_pos); @@ -882,10 +898,12 @@ unsigned int gams::utility::ros::RosParser::get_array_size(std::string var_name, auto cached = ros_array_sizes_.find(array_name); if (cached != ros_array_sizes_.end()) { + // We already determined the arraylength in this message len = cached->second; } else { + // Count the members to determine the length of the array for ( auto it : *array) { std::string key = it.first; @@ -905,6 +923,9 @@ void gams::utility::ros::RosParser::parse_any ( std::string datatype, std::vector & parser_buffer, std::string container_name) { + // This method uses ros_type_introspection to dynamically map values from + // ros messages to capnproto schemas + std::string schema_name = capnp_types_[datatype]; capnp::MallocMessageBuilder buffer; capnp::DynamicStruct::Builder capnp_builder; @@ -962,8 +983,13 @@ void gams::utility::ros::RosParser::parse_any ( std::string datatype, { // Value is NAN if it is not readable } + // Apply name substitution rules from the mapfile name = substitute_name(datatype, name); - if (name == IGNORE_MARKER) continue; + if (name == IGNORE_MARKER) + { + // This member is marked as to be ignored + continue; + } try { set_dyn_capnp_value(capnp_builder, name, val, array_size); @@ -971,7 +997,8 @@ void gams::utility::ros::RosParser::parse_any ( std::string datatype, catch(kj::Exception ex) { std::cout << "Failed to set " << name << " from topic " << - topic_name << " (" << datatype << ")! (" << std::string(ex.getDescription()) << ")" << std::endl; + topic_name << " (" << datatype << ")! (" << + std::string(ex.getDescription()) << ")" << std::endl; } } @@ -982,11 +1009,16 @@ void gams::utility::ros::RosParser::parse_any ( std::string datatype, int array_size = get_array_size(key, &renamed_values); - auto val = strdup(value.c_str()); std::string name = key.substr (topic_len); + + // Apply name substitution rules from the mapfile name = substitute_name(datatype, name); - if (name == IGNORE_MARKER) continue; + if (name == IGNORE_MARKER) + { + // This member is marked as to be ignored + continue; + } try { set_dyn_capnp_value(capnp_builder, name, val, array_size); @@ -994,12 +1026,18 @@ void gams::utility::ros::RosParser::parse_any ( std::string datatype, catch(kj::Exception ex) { std::cout << "Failed to set " << name << " from topic " << - topic_name << " (" << datatype << ")! (" << std::string(ex.getDescription()) << ")" << std::endl; + topic_name << " (" << datatype << ")! (" << + std::string(ex.getDescription()) << ")" << std::endl; } } + // Clear the array size cache ros_array_sizes_.clear(); + + // Write to the Any object madara::knowledge::GenericCapnObject any(schema_name.c_str(), buffer); + // Check if this has to be stored in CircularBuffers + // TODO: remove this once the NativeCircularBuffers ar ready for use auto search = circular_container_stats_.find(container_name); if (search != circular_container_stats_.end()) { @@ -1027,6 +1065,9 @@ void gams::utility::ros::RosParser::parse_any ( std::string datatype, } } +/** + * Wrapper for parse_any so that it can be called from ShapeShifter objects + **/ void gams::utility::ros::RosParser::parse_any (std::string topic, const topic_tools::ShapeShifter & m, @@ -1044,6 +1085,10 @@ void gams::utility::ros::RosParser::parse_any (std::string topic, parse_any(m.getDataType(), topic, parser_buffer, container_name); } +/** + * Wrapper for parse_any so that it can be called from rosbag MessageInstance + * objects + **/ void gams::utility::ros::RosParser::parse_any (const rosbag::MessageInstance & m, std::string container_name) { @@ -1061,7 +1106,10 @@ void gams::utility::ros::RosParser::parse_any (const rosbag::MessageInstance & m parse_any(m.getDataType(), topic, parser_buffer, container_name); } -void gams::utility::ros::RosParser::set_sim_time(global_ros::Time rostime) +/** + * Update the simtime from a ros Time + **/ +void gams::utility::ros::RosParser::set_sim_time (global_ros::Time rostime) { #ifdef MADARA_FEATURE_SIMTIME uint64_t sim_time = rostime.sec * 1e9 + rostime.nsec; @@ -1069,13 +1117,19 @@ void gams::utility::ros::RosParser::set_sim_time(global_ros::Time rostime) #endif } -void gams::utility::ros::RosParser::register_rename_rules( std::map> name_substitution_map) { name_substitution_map_ = std::move(name_substitution_map); } -std::string gams::utility::ros::RosParser::substitute_name(std::string type, +/** + * Name substitusion for any type conversion + **/ +std::string gams::utility::ros::RosParser::substitute_name (std::string type, std::string name) { //Check if this is an array From dc41fb55f9f7a3159d7264b846bd414facccf92e Mon Sep 17 00:00:00 2001 From: Jakob Auer Date: Sun, 23 Sep 2018 10:36:32 +0200 Subject: [PATCH 05/10] fixed problems with capnproto enums and wrong agent prefix in ros2gams --- src/gams/utility/ros/RosParser.cpp | 40 +++++++++++- src/gams/utility/ros/RosParser.h | 45 +++++++++---- tests/test_ros2gams.cpp | 100 ++++++++++++++++++++++++++++- 3 files changed, 168 insertions(+), 17 deletions(-) diff --git a/src/gams/utility/ros/RosParser.cpp b/src/gams/utility/ros/RosParser.cpp index cd131007a..ad76fdbcb 100644 --- a/src/gams/utility/ros/RosParser.cpp +++ b/src/gams/utility/ros/RosParser.cpp @@ -510,9 +510,9 @@ void gams::utility::ros::RosParser::parse_tf_message (tf2_msgs::TFMessage * tf) try { gams::pose::Pose base_pose = base.origin ().transform_to (world); - containers::NativeDoubleVector location ("agents.0.location", + containers::NativeDoubleVector location ("agent.0.location", *knowledge_, 3, eval_settings_); - containers::NativeDoubleVector orientation ("agents.0.orientation", + containers::NativeDoubleVector orientation ("agent.0.orientation", *knowledge_, 3, eval_settings_); location.set (0, base_pose.as_location_vec ().get (0), eval_settings_); location.set (1, base_pose.as_location_vec ().get (1), eval_settings_); @@ -803,6 +803,37 @@ capnp::DynamicStruct::Builder gams::utility::ros::RosParser::get_dyn_capnp_struc } +template +void gams::utility::ros::RosParser::set_dyn_capnp_enum_value ( + capnp::DynamicStruct::Builder dynvalue, + std::string var_name, T val) +{ + dynvalue.set(var_name, val); +} + +// The template specialication has to be defined in these namespace structure +// because of a gcc bug. +// See https://gcc.gnu.org/bugzilla/show_bug.cgi?id=56480 +namespace gams +{ + namespace utility + { + namespace ros + { + void set_dyn_capnp_enum_value( + capnp::DynamicStruct::Builder dynvalue, + std::string var_name, char* val) + { + // We need to check for the enumerants + auto enum_schema = + dynvalue.get(var_name).as().getSchema(); + auto enumerant = enum_schema.getEnumerantByName(kj::StringPtr(val)); + dynvalue.set(var_name, enumerant.getOrdinal()); + } + } + } +} + /* Sets the value of a specifiec schema member */ @@ -869,6 +900,11 @@ void gams::utility::ros::RosParser::set_dyn_capnp_value( bool bool_val = (bool) val; dynvalue.set(var_name, bool_val); } + else if (dynvalue.asReader().get(var_name).getType() == + capnp::DynamicValue::ENUM) + { + set_dyn_capnp_enum_value(dynvalue, var_name, val); + } else { dynvalue.set(var_name, val); diff --git a/src/gams/utility/ros/RosParser.h b/src/gams/utility/ros/RosParser.h index dcfc212a3..a7e30bf23 100644 --- a/src/gams/utility/ros/RosParser.h +++ b/src/gams/utility/ros/RosParser.h @@ -84,7 +84,8 @@ namespace gams static std::string cleanCapnpSchemaName(const std::string& full_name) { std::regex re("[^\\/:]+"); - std::sregex_token_iterator begin(full_name.begin(), full_name.end(), re), end; + std::sregex_token_iterator begin(full_name.begin(), + full_name.end(), re), end; std::vector tokens; std::copy(begin, end, std::back_inserter(tokens)); return tokens.back(); @@ -98,9 +99,11 @@ namespace gams RosParser (knowledge::KnowledgeBase * kb, std::string world_frame, std::string base_frame, std::map capnp_types, - std::map circular_containers=std::map(), - knowledge::EvalSettings eval_settings=knowledge::EvalSettings(), - std::string frame_prefix=gams::pose::ReferenceFrame::default_prefix()); + std::map circular_containers=std::map(), + knowledge::EvalSettings eval_settings=knowledge::EvalSettings(), + std::string frame_prefix= + gams::pose::ReferenceFrame::default_prefix()); void parse_message (const rosbag::MessageInstance m, std::string container_name); void parse_message (const topic_tools::ShapeShifter::ConstPtr& m, @@ -119,7 +122,9 @@ namespace gams * Registers renaming rules for ros types * @param name_substitution_map map> **/ - void register_rename_rules( std::map> name_substitution_map); + void register_rename_rules ( + std::map> name_substitution_map); /** * Parse defined Any types @@ -242,23 +247,39 @@ namespace gams - - capnp::DynamicStruct::Builder get_dyn_capnp_struct( + /** + * Searches for the appropriate capnproto "substruct" builder defined + * by the name + **/ + capnp::DynamicStruct::Builder get_dyn_capnp_struct ( capnp::DynamicStruct::Builder builder, std::string name); + /** + * Sets the value of an enum in a capnproto struct + **/ template - void set_dyn_capnp_value(capnp::DynamicStruct::Builder builder, - std::string name, T val, unsigned int array_size); + void set_dyn_capnp_enum_value (capnp::DynamicStruct::Builder dynvalue, + std::string var_name, T val); - //template - unsigned int get_array_size(std::string var_name, + /** + * Sets a value in a capnproto struct + **/ + template + void set_dyn_capnp_value (capnp::DynamicStruct::Builder builder, + std::string name, T val, unsigned int array_size); + + /** + * Calculates the size of an array in ros type introspection + * generated values + **/ + unsigned int get_array_size (std::string var_name, RosIntrospection::RenamedValues* array); /** * Substitutes the names of topic type members based on the registered * rules **/ - std::string substitute_name(std::string type, std::string name); + std::string substitute_name (std::string type, std::string name); /* Sets the current time to the ros header time if the simtime feature is diff --git a/tests/test_ros2gams.cpp b/tests/test_ros2gams.cpp index a683a581e..90fc77175 100644 --- a/tests/test_ros2gams.cpp +++ b/tests/test_ros2gams.cpp @@ -23,6 +23,8 @@ #include "std_msgs/Header.h" #include "nav_msgs/Odometry.h" #include "sensor_msgs/RegionOfInterest.h" +#include "sensor_msgs/CompressedImage.h" + const double TEST_epsilon = 0.0001; int gams_fails = 0; @@ -428,14 +430,106 @@ void test_any_odom() TEST(pose_covariance[17].as(), cov); } + +void test_any_compressed_image() +{ + // This tests configures the parser so that Odometry messages are stored + // into capnproto any types. + + std::cout << std::endl << std::endl << "test_any_compressed_image" + << std::endl << std::endl; + + std::string container_name = "thisisatest"; + + sensor_msgs::CompressedImage image; + image.header.frame_id = "img"; + + image.format = "jpeg"; + int size = 100; + std::vector data_reference; + for ( int i = 0; i < size; ++i) + { + uint8_t r = rand() % 255; + image.data.push_back(r); + data_reference.push_back(r); + } + + // transform to shapeshifter object + topic_tools::ShapeShifter shape_shifter; + shape_shifter.morph( + ros::message_traits::MD5Sum::value(), + ros::message_traits::DataType::value(), + ros::message_traits::Definition::value(), + "" ); + + std::vector buffer; + serialize_message_to_array(image, buffer); + ros::serialization::OStream stream( buffer.data(), buffer.size() ); + shape_shifter.read( stream ); + + + // The parser + std::map typemap; + typemap["sensor_msgs/CompressedImage"] = "CompressedImage"; + + knowledge::KnowledgeBase knowledge; + gams::utility::ros::RosParser parser (&knowledge, "", "", typemap); + + // register the type using the topic_name as identifier. This allows us to + // use RosIntrospection + const std::string topic_name = "img"; + const std::string datatype = shape_shifter.getDataType(); + const std::string definition = shape_shifter.getMessageDefinition(); + parser.registerMessageDefinition (topic_name, + RosIntrospection::ROSType (datatype), definition); + + // Load the schemas from disk + auto path = madara::utility::expand_envs( + "$(GAMS_ROOT)/src/gams/types/CompressedImage.capnp.bin"); + parser.load_capn_schema(path); + parser.print_schemas(); + + // parse to capnp format + parser.parse_any(topic_name, shape_shifter, container_name); + + knowledge.print(); + + madara::knowledge::GenericCapnObject any = + knowledge.get(container_name).to_any(); + + int fd = open(path.c_str(), 0, O_RDONLY); + capnp::StreamFdMessageReader schema_message_reader(fd); + auto schema_reader = schema_message_reader.getRoot(); + capnp::SchemaLoader loader; + + std::map schemas; + + for (auto schema : schema_reader.getNodes()) { + schemas[gams::utility::ros::cleanCapnpSchemaName (schema.getDisplayName ())] = + loader.load(schema); + } + auto capn_img = + any.reader(schemas[typemap["sensor_msgs/CompressedImage"]].asStruct ()); + + auto data = capn_img.get ("data").as (); + TEST (data.size(), data_reference.size()); + for ( int i = 0; i < size; ++i) + { + TEST(image.data[i], data_reference[i]); + } + auto format = capn_img.get("format").as().cStr(); + std::cout << "Format: " << format << std::endl; +} + int main (int, char **) { std::cout << "Testing ros2gams" << std::endl; //test_pose(); //test_tf_tree(); - test_any_point(); - test_any_bool(); - test_any_odom(); + //test_any_point(); + //test_any_bool(); + //test_any_odom(); + test_any_compressed_image(); if (gams_fails > 0) { From 4059a0af0c92edfd06ad4227dcf41597c8307726 Mon Sep 17 00:00:00 2001 From: Jakob Auer Date: Sun, 23 Sep 2018 15:39:48 +0200 Subject: [PATCH 06/10] added progressbar to ros2gams --- src/gams/programs/ros2gams.cpp | 33 +++++++++++++++++++++++++----- src/gams/utility/ros/RosParser.cpp | 15 +++++++------- 2 files changed, 36 insertions(+), 12 deletions(-) diff --git a/src/gams/programs/ros2gams.cpp b/src/gams/programs/ros2gams.cpp index 2d6524362..897ddb685 100644 --- a/src/gams/programs/ros2gams.cpp +++ b/src/gams/programs/ros2gams.cpp @@ -384,10 +384,6 @@ int main (int argc, char ** argv) RosIntrospection::ROSType (datatype), definition); } - /*std::vector rules; - rules.push_back( RosIntrospection::SubstitutionRule("angle_min", "angle_min", "aaa@") ); - parser.registerRenamingRules( RosIntrospection::ROSType ("sensor_msgs/LaserScan"), rules);*/ - // Name substitution parser.register_rename_rules(name_substitution_map); // Load the capnproto schemas @@ -431,6 +427,9 @@ int main (int argc, char ** argv) // Iterate through all topics in the bagfile std::cout << "Converting...\n"; + float progress; + int message_index = 0; + int progress_bar_width = 70; for (const rosbag::MessageInstance m: view) { std::string topic = m.getTopic (); @@ -502,7 +501,31 @@ int main (int argc, char ** argv) } settings.last_lamport_clock += 1; + // Printing the progress bar + // See https://stackoverflow.com/questions/14539867/how-to-display-a-progress-indicator-in-pure-c-c-cout-printf + message_index++; + progress = message_index / float(view.size ()); + std::cout << "["; + int pos = progress_bar_width * progress; + for (int i = 0; i < progress_bar_width; ++i) { + if (i < pos) + { + std::cout << "="; + } + else if (i == pos) + { + std::cout << ">"; + } + else + { + std::cout << " "; + } + } + std::cout << "] " << int(progress * 100.0) << " % " << message_index << "/" + << view.size() << "\r"; + std::cout.flush(); } + std::cout << std::endl << "done" << std::endl; // Storing stats in the manifest knowledge manifest.set ("last_timestamp", (Integer) settings.last_timestamp); @@ -513,7 +536,7 @@ int main (int argc, char ** argv) manifest.set ("initial_lamport_clock", (Integer) settings.initial_lamport_clock); manifest.set ("last_checkpoint_id", checkpoint_id-1); - manifest.save_as_karl(checkpoint_prefix + "_manifest.kb"); + manifest.save_as_karl(checkpoint_prefix + ".manifest.kb"); } #endif diff --git a/src/gams/utility/ros/RosParser.cpp b/src/gams/utility/ros/RosParser.cpp index ad76fdbcb..7377dc6a1 100644 --- a/src/gams/utility/ros/RosParser.cpp +++ b/src/gams/utility/ros/RosParser.cpp @@ -750,14 +750,14 @@ std::string gams::utility::ros::ros_to_gams_name (std::string ros_topic_name) void gams::utility::ros::RosParser::load_capn_schema(std::string path) { try{ - int fd = open(path.c_str(), 0, O_RDONLY); - capnp::StreamFdMessageReader schema_message_reader(fd); + int fd = open (path.c_str (), 0, O_RDONLY); + capnp::StreamFdMessageReader schema_message_reader (fd); auto schema_reader = - schema_message_reader.getRoot(); + schema_message_reader.getRoot (); - for (auto schema : schema_reader.getNodes()) { - std::string schema_name = cleanCapnpSchemaName (schema.getDisplayName()); - schemas_[schema_name] = capnp_loader_.load(schema); + for (auto schema : schema_reader.getNodes ()) { + std::string schema_name = cleanCapnpSchemaName (schema.getDisplayName ()); + schemas_[schema_name] = capnp_loader_.load (schema); } } catch(...) @@ -973,7 +973,8 @@ void gams::utility::ros::RosParser::parse_any ( std::string datatype, } catch(...) { - std::cout << "Schema with name " << schema_name << "not found!" << std::endl; + std::cout << "Schema with name " << schema_name << "not found!" + << std::endl; exit(1); } From 0fb50904a02324071208009a277fef51ecbdf4ec Mon Sep 17 00:00:00 2001 From: Jakob Auer Date: Sun, 23 Sep 2018 17:09:07 +0200 Subject: [PATCH 07/10] metadata in checkpoints optional for ros2gams and added PointCloudXYZ --- src/gams/programs/ros2gams.cpp | 33 +++++++++++++++++------------- src/gams/types/PointCloud.capnp | 19 ----------------- src/gams/types/PointCloud2.capnp | 24 ---------------------- src/gams/utility/ros/RosParser.cpp | 9 ++++++-- 4 files changed, 26 insertions(+), 59 deletions(-) delete mode 100644 src/gams/types/PointCloud.capnp delete mode 100644 src/gams/types/PointCloud2.capnp diff --git a/src/gams/programs/ros2gams.cpp b/src/gams/programs/ros2gams.cpp index 897ddb685..8be106154 100644 --- a/src/gams/programs/ros2gams.cpp +++ b/src/gams/programs/ros2gams.cpp @@ -94,6 +94,9 @@ bool save_as_json = false; bool save_as_binary = false; bool save_as_stream = false; +//Metadata in the checkpoints +bool store_metadata = false; + //Buffer size int write_buffer_size = 10240000; @@ -137,6 +140,10 @@ void handle_arguments (int argc, char ** argv) checkpoint_prefix = argv[i + 1]; i++; } + else if (arg1 == "-meta") + { + store_metadata = true; + } else if (arg1 == "-m" || arg1 == "--map-file") { if (i + 1 < argc) @@ -215,6 +222,7 @@ void handle_arguments (int argc, char ** argv) " base as a karl checkpoint"\ "(default)\n" \ " [-m|--map-file file] File with filter information\n" \ + " [-meta] stores metadata in the checkpoints\n" \ " [-y|--frequency hz] Checkpoint frequency\n" \ " (default:checkpoint with each\n" \ " message in the bagfile)\n" \ @@ -549,20 +557,17 @@ int main (int argc, char ** argv) int save_checkpoint (knowledge::KnowledgeBase * knowledge, knowledge::CheckpointSettings * settings, std::string meta_prefix) { - /*knowledge->set (meta_prefix + ".originator", - settings->originator); - knowledge->set (meta_prefix + ".version", - settings->version); - knowledge->set (meta_prefix + ".current_timestamp", - (Integer) time (NULL));*/ - knowledge->set (meta_prefix + ".last_timestamp", - (Integer) settings->last_timestamp); - knowledge->set (meta_prefix + ".initial_timestamp", - (Integer) settings->initial_timestamp); - knowledge->set (meta_prefix + ".last_lamport_clock", - (Integer) settings->last_lamport_clock); - knowledge->set (meta_prefix + ".initial_lamport_clock", - (Integer) settings->initial_lamport_clock); + if ( store_metadata ) + { + knowledge->set (meta_prefix + ".last_timestamp", + (Integer) settings->last_timestamp); + knowledge->set (meta_prefix + ".initial_timestamp", + (Integer) settings->initial_timestamp); + knowledge->set (meta_prefix + ".last_lamport_clock", + (Integer) settings->last_lamport_clock); + knowledge->set (meta_prefix + ".initial_lamport_clock", + (Integer) settings->initial_lamport_clock); + } int ret = 0; std::string filename = settings->filename; diff --git a/src/gams/types/PointCloud.capnp b/src/gams/types/PointCloud.capnp deleted file mode 100644 index 75127b2bc..000000000 --- a/src/gams/types/PointCloud.capnp +++ /dev/null @@ -1,19 +0,0 @@ -# Generated by ./generate_schemas.py. This file should not be modified by hand. -@0xe7a0adc785220d2a; - -# Namespace setup -using Cxx = import "/capnp/c++.capnp"; -$Cxx.namespace("gams::types"); - -# Capnfile Imports -using import "Point32.capnp".Point32; -using import "Header.capnp".Header; -using import "ChannelFloat32.capnp".ChannelFloat32; - -# Type definition -struct PointCloud { - channels @0: List(ChannelFloat32); - header @1: Header; - points @2: List(Point32); - -} diff --git a/src/gams/types/PointCloud2.capnp b/src/gams/types/PointCloud2.capnp deleted file mode 100644 index 2f146a955..000000000 --- a/src/gams/types/PointCloud2.capnp +++ /dev/null @@ -1,24 +0,0 @@ -# Generated by ./generate_schemas.py. This file should not be modified by hand. -@0xd05b35a2845585f4; - -# Namespace setup -using Cxx = import "/capnp/c++.capnp"; -$Cxx.namespace("gams::types"); - -# Capnfile Imports -using import "PointField.capnp".PointField; -using import "Header.capnp".Header; - -# Type definition -struct PointCloud2 { - width @0: UInt32; - isDense @1: Bool; - pointStep @2: UInt32; - height @3: UInt32; - header @4: Header; - data @5: List(UInt8); - fields @6: List(PointField); - rowStep @7: UInt32; - isBigendian @8: Bool; - -} diff --git a/src/gams/utility/ros/RosParser.cpp b/src/gams/utility/ros/RosParser.cpp index 7377dc6a1..1d5174225 100644 --- a/src/gams/utility/ros/RosParser.cpp +++ b/src/gams/utility/ros/RosParser.cpp @@ -509,7 +509,12 @@ void gams::utility::ros::RosParser::parse_tf_message (tf2_msgs::TFMessage * tf) } try { - gams::pose::Pose base_pose = base.origin ().transform_to (world); + auto base_origin = base.origin (); + if (!base_origin.is_set () || !world.valid ()) + { + return; + } + gams::pose::Pose base_pose = base_origin.transform_to (world); containers::NativeDoubleVector location ("agent.0.location", *knowledge_, 3, eval_settings_); containers::NativeDoubleVector orientation ("agent.0.orientation", @@ -525,7 +530,7 @@ void gams::utility::ros::RosParser::parse_tf_message (tf2_msgs::TFMessage * tf) eval_settings_); } - catch ( gams::pose::unrelated_frames ex){} + catch (gams::pose::unrelated_frames ex){} } } From b96942b327966a4fd7bd5c1980dbc840101ae28b Mon Sep 17 00:00:00 2001 From: Jakob Auer Date: Sun, 23 Sep 2018 20:34:04 +0200 Subject: [PATCH 08/10] PointCloud PCL support for ros2gams --- scripts/linux/base_build.sh | 2 +- src/gams/programs/ros2gams.cpp | 3 +- src/gams/utility/ros/RosParser.cpp | 76 +++++++++++++++++++++++++++++- src/gams/utility/ros/RosParser.h | 15 ++++++ using_ros.mpb | 4 +- 5 files changed, 95 insertions(+), 5 deletions(-) diff --git a/scripts/linux/base_build.sh b/scripts/linux/base_build.sh index 739463c29..07fd0c3aa 100755 --- a/scripts/linux/base_build.sh +++ b/scripts/linux/base_build.sh @@ -530,7 +530,7 @@ if [ $PREREQS -eq 1 ] && [ $MAC -eq 0 ]; then fi #android condition ends if [ $ROS -eq 1 ]; then - sudo apt-get install -y ros-kinetic-desktop-full python-rosinstall ros-kinetic-ros-type-introspection ros-kinetic-move-base-msgs ros-kinetic-navigation libactionlib-dev libactionlib-msgs-dev libmove-base-msgs-dev + sudo apt-get install -y ros-kinetic-desktop-full python-rosinstall ros-kinetic-ros-type-introspection ros-kinetic-move-base-msgs ros-kinetic-navigation libactionlib-dev libactionlib-msgs-dev libmove-base-msgs-dev ros-kinetic-pcl-conversions ros-kinetic-pcl-ros libpcl-dev if [ $ROS_FIRST_SETUP -eq 1 ]; then sudo rosdep init diff --git a/src/gams/programs/ros2gams.cpp b/src/gams/programs/ros2gams.cpp index 8be106154..e0aa79c72 100644 --- a/src/gams/programs/ros2gams.cpp +++ b/src/gams/programs/ros2gams.cpp @@ -98,7 +98,7 @@ bool save_as_stream = false; bool store_metadata = false; //Buffer size -int write_buffer_size = 10240000; +int write_buffer_size = 1024*1024*100; // the world and the base frame of the robot std::string base_frame = ""; @@ -425,6 +425,7 @@ int main (int argc, char ** argv) { madara::knowledge::CheckpointSettings stream_settings; stream_settings.filename = stream_file + ".stk"; + stream_settings.buffer_size = write_buffer_size; delete_existing_file(stream_settings.filename, delete_existing); //kb.attach_streamer(std::move(stream_settings), kb, 100); diff --git a/src/gams/utility/ros/RosParser.cpp b/src/gams/utility/ros/RosParser.cpp index 1d5174225..fc50e0e9b 100644 --- a/src/gams/utility/ros/RosParser.cpp +++ b/src/gams/utility/ros/RosParser.cpp @@ -43,7 +43,18 @@ void gams::utility::ros::RosParser::parse_message ( { // If the message type is in the mapfile we have to parse this message // into a capnproto schema - parse_any(m, container_name); + + if (m.isType () && + capnp_types_["sensor_msgs/PointCloud2"] == "pcl") + { + parse_pointcloud2_pclschema ( + m.instantiate ().get (), + container_name); + } + else + { + parse_any(m, container_name); + } } else if (m.isType ()) { @@ -106,6 +117,8 @@ void gams::utility::ros::RosParser::parse_message ( const topic_tools::ShapeShifter::ConstPtr& m, std::string container_name) { + // TODO: HANDLE CAPNP ANY TYPES!!! + if (m->getDataType () == "std_msgs/Int32") { int value = m->instantiate()->data; @@ -1148,13 +1161,72 @@ void gams::utility::ros::RosParser::parse_any (const rosbag::MessageInstance & m parse_any(m.getDataType(), topic, parser_buffer, container_name); } +void gams::utility::ros::RosParser::parse_pointcloud2_pclschema ( + sensor_msgs::PointCloud2 * pointcloud, + std::string container_name) +{ + // THIS METHOD IS HARDCODED TO RUN WITH PCL BASED SCHEMAS + + // Load PCL schema + std::string pointcloud_schema_name = "PointCloudXYZ"; + capnp::MallocMessageBuilder buffer; + capnp::DynamicStruct::Builder pointcloud_builder; + try + { + auto pointcloud_schema = schemas_.at(pointcloud_schema_name).asStruct(); + pointcloud_builder = buffer.initRoot(pointcloud_schema); + madara::knowledge::Any::register_schema(pointcloud_schema_name.c_str(), + pointcloud_schema); + } + catch(...) + { + std::cout << "Schema with name " << pointcloud_schema_name << + "not found!" << std::endl; + exit(1); + } + + // Convert from ROS PointCloud2 to a PCL PointCloud + pcl::PCLPointCloud2 pcl_pc2; + pcl_conversions::toPCL (*pointcloud, pcl_pc2); + pcl::PointCloud::Ptr temp_cloud (new pcl::PointCloud); + pcl::fromPCLPointCloud2 (pcl_pc2, *temp_cloud); + + //Fill the list of points with data + auto point_list = pointcloud_builder.init ("points", + (*temp_cloud).size ()).as(); + int point_counter = 0; + for (pcl::PointXYZ point : *temp_cloud) + { + auto capnp_point = point_list[point_counter].as (); + capnp_point.set("x", point.x); + capnp_point.set("y", point.y); + capnp_point.set("z", point.z); + point_counter++; + } + + // Now fill the other fields + pointcloud_builder.set ("tov", + ((unsigned long)pointcloud->header.stamp.sec * 1e9) + + (unsigned long)pointcloud->header.stamp.nsec); + pointcloud_builder.set ("frameId", pointcloud->header.frame_id.c_str()); + pointcloud_builder.set ("width", pointcloud->width); + pointcloud_builder.set ("height", pointcloud->height); + pointcloud_builder.set ("isDense", (bool)pointcloud->is_dense); + + // Store in the knowledgebase + madara::knowledge::GenericCapnObject any(pointcloud_schema_name.c_str(), + buffer); + knowledge_->set_any(container_name, any); +} + /** * Update the simtime from a ros Time **/ void gams::utility::ros::RosParser::set_sim_time (global_ros::Time rostime) { #ifdef MADARA_FEATURE_SIMTIME - uint64_t sim_time = rostime.sec * 1e9 + rostime.nsec; + uint64_t sim_time = (unsigned long)rostime.sec * 1e9 + + (unsigned long)rostime.nsec; madara::utility::sim_time_notify(sim_time, 0.0); #endif } diff --git a/src/gams/utility/ros/RosParser.h b/src/gams/utility/ros/RosParser.h index a7e30bf23..8bce5bf42 100644 --- a/src/gams/utility/ros/RosParser.h +++ b/src/gams/utility/ros/RosParser.h @@ -28,9 +28,11 @@ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" #pragma GCC diagnostic ignored "-Wreorder" +#pragma GCC diagnostic ignored "-Wnon-virtual-dtor" #endif #include "ros/ros.h" +#include "ros/time.h" #include "ros/serialization.h" #include "rosbag/bag.h" #include "rosbag/view.h" @@ -38,6 +40,11 @@ #include #include +#include +#include +#include +#include + #include #include @@ -59,6 +66,7 @@ #pragma GCC diagnostic pop #endif +#include #include #include #include @@ -161,6 +169,13 @@ namespace gams * @param path string with the path to the schema file **/ void load_capn_schema(std::string path); + + /** + * Parses a ros PointCloud2 into a PCL based capnp schema + **/ + void parse_pointcloud2_pclschema (sensor_msgs::PointCloud2 * pointcloud, + std::string container_name); + //known types void parse_odometry (nav_msgs::Odometry * odom, diff --git a/using_ros.mpb b/using_ros.mpb index aa6da9fdc..db2e11b37 100644 --- a/using_ros.mpb +++ b/using_ros.mpb @@ -5,6 +5,8 @@ feature (ros) { includes += $(ROS_ROOT)/../../include includes += /usr/include/libcxxabi + includes += /usr/include/pcl-1.7/ libpaths += $(ROS_ROOT)/../../lib - libs += rosconsole actionlib rosbag_storage rosbag rostime roscpp_serialization cpp_common roscpp roslib ros_type_introspection topic_tools tf2_ros + libpaths += /usr/lib/x86_64-linux-gnu + libs += rosconsole actionlib rosbag_storage rosbag rostime roscpp_serialization cpp_common roscpp roslib ros_type_introspection topic_tools tf2_ros pcl_common } From 8c928cd168a6ae006aa5ee927eb66ce192d99801 Mon Sep 17 00:00:00 2001 From: Jakob Auer Date: Mon, 24 Sep 2018 16:19:53 +0200 Subject: [PATCH 09/10] improved manifest file handling and contents for ros2gams --- scripts/linux/zip_dep_bin.sh | 2 +- src/gams/pose/ReferenceFrame.inl | 11 +++++-- src/gams/programs/ros2gams.cpp | 51 +++++++++++++++++++++-------- src/gams/types/PointCloudXYZ.capnp | 20 +++++++++++ src/gams/types/PointCloudXYZI.capnp | 21 ++++++++++++ src/gams/types/PointXYZ.capnp | 15 +++++++++ src/gams/types/PointXYZI.capnp | 14 ++++++++ src/gams/utility/ros/RosParser.cpp | 4 --- 8 files changed, 118 insertions(+), 20 deletions(-) create mode 100644 src/gams/types/PointCloudXYZ.capnp create mode 100644 src/gams/types/PointCloudXYZI.capnp create mode 100644 src/gams/types/PointXYZ.capnp create mode 100644 src/gams/types/PointXYZI.capnp diff --git a/scripts/linux/zip_dep_bin.sh b/scripts/linux/zip_dep_bin.sh index 28c830dc5..5b93260aa 100755 --- a/scripts/linux/zip_dep_bin.sh +++ b/scripts/linux/zip_dep_bin.sh @@ -22,7 +22,7 @@ if [ $PRINT_USAGE -eq 0 ] ; then SCRIPT=${BIN}.sh ZIP=${BIN}.zip - DEPS=$(ldd $BIN | cut -d' ' -f3 | egrep "(/opt|/home)") + DEPS=$(ldd $BIN | cut -d' ' -f3 | egrep "(/opt|/home|pcl)") mkdir -p $DIR diff --git a/src/gams/pose/ReferenceFrame.inl b/src/gams/pose/ReferenceFrame.inl index 691116f02..42aea3fd0 100644 --- a/src/gams/pose/ReferenceFrame.inl +++ b/src/gams/pose/ReferenceFrame.inl @@ -415,9 +415,16 @@ inline void transform( CoordType &in, const ReferenceFrame &to_frame) { - if (to_frame == in.frame()) + if (to_frame == in.frame()) { return; - else if (to_frame == in.frame().origin_frame()) + } + if (!to_frame.valid()) { + throw unrelated_frames(in.frame(), to_frame); + } + if (!in.frame().valid()) { + throw unrelated_frames(in.frame(), to_frame); + } + if (to_frame == in.frame().origin_frame()) { transform_to_origin(in); in.frame(in.frame().origin_frame()); diff --git a/src/gams/programs/ros2gams.cpp b/src/gams/programs/ros2gams.cpp index e0aa79c72..ce512828d 100644 --- a/src/gams/programs/ros2gams.cpp +++ b/src/gams/programs/ros2gams.cpp @@ -65,7 +65,6 @@ #pragma GCC diagnostic pop #endif - namespace logger = madara::logger; namespace knowledge = madara::knowledge; namespace containers = knowledge::containers; @@ -80,7 +79,7 @@ std::string rosbag_path = ""; //std::string rosbag_robot_prefix = "robot_"; // prefix for the madara checkoints -std::string checkpoint_prefix = "checkpoint_"; +std::string checkpoint_prefix = "checkpoint"; // path to the filter definition file std::string map_file = ""; @@ -88,6 +87,9 @@ std::string map_file = ""; //path to the checkpoint stream file std::string stream_file = ""; +//path to the manifest file +std::string manifest_file = ""; + // save as a karl or binary file bool save_as_karl = false; bool save_as_json = false; @@ -161,6 +163,14 @@ void handle_arguments (int argc, char ** argv) } i++; } + else if (arg1 == "-mp" || arg1 == "--manifest-path") + { + if (i + 1 < argc) + { + manifest_file = argv[i + 1]; + } + i++; + } else if (arg1 == "-rp" || arg1 == "--ros-robot-prefix") { //rosbag_robot_prefix = argv[i + 1]; @@ -223,6 +233,7 @@ void handle_arguments (int argc, char ** argv) "(default)\n" \ " [-m|--map-file file] File with filter information\n" \ " [-meta] stores metadata in the checkpoints\n" \ + " [-mp|--manifest-path file] path to the manifest file\n" \ " [-y|--frequency hz] Checkpoint frequency\n" \ " (default:checkpoint with each\n" \ " message in the bagfile)\n" \ @@ -421,9 +432,10 @@ int main (int argc, char ** argv) } // Attach streaming + + madara::knowledge::CheckpointSettings stream_settings; if (save_as_stream) { - madara::knowledge::CheckpointSettings stream_settings; stream_settings.filename = stream_file + ".stk"; stream_settings.buffer_size = write_buffer_size; delete_existing_file(stream_settings.filename, delete_existing); @@ -536,16 +548,29 @@ int main (int argc, char ** argv) } std::cout << std::endl << "done" << std::endl; - // Storing stats in the manifest knowledge - manifest.set ("last_timestamp", (Integer) settings.last_timestamp); - manifest.set ("initial_timestamp", (Integer) settings.initial_timestamp); - manifest.set ("duration_ns", (Integer) - ( settings.last_timestamp - settings.initial_timestamp)); - manifest.set ("last_lamport_clock", (Integer) settings.last_lamport_clock-1); - manifest.set ("initial_lamport_clock", - (Integer) settings.initial_lamport_clock); - manifest.set ("last_checkpoint_id", checkpoint_id-1); - manifest.save_as_karl(checkpoint_prefix + ".manifest.kb"); + if (manifest_file != "") + { + // Storing stats in the manifest knowledge + manifest.set ("last_timestamp", (Integer) settings.last_timestamp); + manifest.set ("initial_timestamp", (Integer) settings.initial_timestamp); + manifest.set ("last_lamport_clock", (Integer) settings.last_lamport_clock-1); + manifest.set ("initial_lamport_clock", + (Integer) settings.initial_lamport_clock); + manifest.set ("last_checkpoint_id", checkpoint_id-1); + + // ROSBAG infos + manifest.set ("name", bag.getFileName()); + ros::Time begin_time = view.getBeginTime (); + ros::Time end_time = view.getEndTime (); + manifest.set ("date", begin_time.toNSec ()); + manifest.set ("duration", (end_time - begin_time).toNSec ()); + if (save_as_stream) + { + manifest.set ("size", + boost::filesystem::file_size(stream_settings.filename)); + } + manifest.save_as_karl (manifest_file + ".mf"); + } } #endif diff --git a/src/gams/types/PointCloudXYZ.capnp b/src/gams/types/PointCloudXYZ.capnp new file mode 100644 index 000000000..229148b6f --- /dev/null +++ b/src/gams/types/PointCloudXYZ.capnp @@ -0,0 +1,20 @@ +@0xcf7419e96f91ec54; + +# Namespace setup +using Cxx = import "/capnp/c++.capnp"; +$Cxx.namespace("gams::types"); + +using import "PointXYZ.capnp".PointXYZ; + +# TODO use capnproto generics (Templates) to unduplicate this and PointCloudXYZI + +# Type definition +struct PointCloudXYZ { + tov @0 :UInt64; + frameId @1 :Text; + width @2 :UInt32; + height @3 :UInt32; + isDense @4 :Bool; + points @5 :List(PointXYZ); + +} diff --git a/src/gams/types/PointCloudXYZI.capnp b/src/gams/types/PointCloudXYZI.capnp new file mode 100644 index 000000000..c22481546 --- /dev/null +++ b/src/gams/types/PointCloudXYZI.capnp @@ -0,0 +1,21 @@ +@0xa62f4365c6b2a67c; + +# Namespace setup +using Cxx = import "/capnp/c++.capnp"; +$Cxx.namespace("gams::types"); + +using import "PointXYZI.capnp".PointXYZI; + +# TODO use capnproto generics (Templates) to unduplicate this and PointCloudXYZ + +# Type definition +struct PointCloudXYZI { + tov @0 :UInt64; + frameId @1 :Text; + width @2 :UInt32; + height @3 :UInt32; + isDense @4 :Bool; + points @5 :List(PointXYZI); + +} + diff --git a/src/gams/types/PointXYZ.capnp b/src/gams/types/PointXYZ.capnp new file mode 100644 index 000000000..134b9ebe9 --- /dev/null +++ b/src/gams/types/PointXYZ.capnp @@ -0,0 +1,15 @@ +@0xb8c9b16fb51cc7cc; + +# Namespace setup +using Cxx = import "/capnp/c++.capnp"; +$Cxx.namespace("gams::types"); + +# Capnfile Imports + +# Type definition +struct PointXYZ { + y @0: Float32; + x @1: Float32; + z @2: Float32; + +} diff --git a/src/gams/types/PointXYZI.capnp b/src/gams/types/PointXYZI.capnp new file mode 100644 index 000000000..97db11386 --- /dev/null +++ b/src/gams/types/PointXYZI.capnp @@ -0,0 +1,14 @@ +@0xc94eebdb0ddcfeb7; + +# Namespace setup +using Cxx = import "/capnp/c++.capnp"; +$Cxx.namespace("gams::types"); + +# Type definition +struct PointXYZI { + y @0 :Float32; + x @1 :Float32; + z @2 :Float32; + intensity @3 :Float32; + +} diff --git a/src/gams/utility/ros/RosParser.cpp b/src/gams/utility/ros/RosParser.cpp index fc50e0e9b..beb0ce32b 100644 --- a/src/gams/utility/ros/RosParser.cpp +++ b/src/gams/utility/ros/RosParser.cpp @@ -523,10 +523,6 @@ void gams::utility::ros::RosParser::parse_tf_message (tf2_msgs::TFMessage * tf) try { auto base_origin = base.origin (); - if (!base_origin.is_set () || !world.valid ()) - { - return; - } gams::pose::Pose base_pose = base_origin.transform_to (world); containers::NativeDoubleVector location ("agent.0.location", *knowledge_, 3, eval_settings_); From fcdf873834dd29fcdd52023971a800fef6c1f939 Mon Sep 17 00:00:00 2001 From: Jakob Auer Date: Mon, 1 Oct 2018 14:18:35 +0200 Subject: [PATCH 10/10] Fixed bug in Datatypes.h for new PointCloud types --- src/gams/types/Datatypes.h | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/gams/types/Datatypes.h b/src/gams/types/Datatypes.h index 67909d912..ab25db09d 100644 --- a/src/gams/types/Datatypes.h +++ b/src/gams/types/Datatypes.h @@ -26,13 +26,15 @@ #include "Joy.capnp.h" #include "LaserScan.capnp.h" #include "MagneticField.capnp.h" -#include "PointCloud.capnp.h" -#include "PointCloud2.capnp.h" +#include "PointCloudXYZ.capnp.h" +#include "PointCloudXYZI.capnp.h" #include "PointField.capnp.h" #include "Temperature.capnp.h" #include "TimeReference.capnp.h" #include "RegionOfInterest.capnp.h" #include "Point.capnp.h" +#include "PointXYZ.capnp.h" +#include "PointXYZI.capnp.h" #include "MapMetaData.capnp.h" #include "Pose.capnp.h" #include "Quaternion.capnp.h" @@ -75,13 +77,15 @@ namespace gams mk::Any::register_type >("Joy"); mk::Any::register_type >("LaserScan"); mk::Any::register_type >("MagneticField"); - mk::Any::register_type >("PointCloud"); - mk::Any::register_type >("PointCloud2"); + mk::Any::register_type >("PointCloudXYZ"); + mk::Any::register_type >("PointCloudXYZI"); mk::Any::register_type >("PointField"); mk::Any::register_type >("Temperature"); mk::Any::register_type >("TimeReference"); mk::Any::register_type >("RegionOfInterest"); mk::Any::register_type >("Point"); + mk::Any::register_type >("PointXYZ"); + mk::Any::register_type >("PointXYZI"); mk::Any::register_type >("MapMetaData"); mk::Any::register_type >("Pose"); mk::Any::register_type >("Quaternion");