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/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/programs/ros2gams.cpp b/src/gams/programs/ros2gams.cpp index 2d6524362..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,14 +87,20 @@ 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; 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; +int write_buffer_size = 1024*1024*100; // the world and the base frame of the robot std::string base_frame = ""; @@ -137,6 +142,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) @@ -154,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]; @@ -215,6 +232,8 @@ 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" \ + " [-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" \ @@ -384,10 +403,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 @@ -417,10 +432,12 @@ 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); //kb.attach_streamer(std::move(stream_settings), kb, 100); @@ -431,6 +448,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,18 +522,55 @@ 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); - 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 @@ -526,20 +583,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/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"); 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/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 cd131007a..beb0ce32b 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; @@ -509,10 +522,11 @@ 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", + auto base_origin = base.origin (); + gams::pose::Pose base_pose = base_origin.transform_to (world); + 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_); @@ -525,7 +539,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){} } } @@ -750,14 +764,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(...) @@ -803,6 +817,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 +914,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); @@ -937,7 +987,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); } @@ -1106,13 +1157,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 dcfc212a3..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 @@ -84,7 +92,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 +107,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 +130,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 @@ -156,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, @@ -242,23 +262,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) { 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 }