Skip to content
Permalink
Browse files

Ros2gams shield schema (#50)

* adaptions in ros2gams to be able to handle list renaming and ignore members

* fixed bugs caused by changes to ReferenceFrame in ros2gams

* performance improvements

* added more comments to rosparser for ros2gams

* fixed problems with capnproto enums and wrong agent prefix in ros2gams

* added progressbar to ros2gams

* metadata in checkpoints optional for ros2gams and added PointCloudXYZ

* PointCloud PCL support for ros2gams

* improved manifest file handling and contents for ros2gams

* Fixed bug in Datatypes.h for new PointCloud types
  • Loading branch information...
auerj authored and dskyle committed Oct 2, 2018
1 parent c2f870e commit 4d4de85264d2d49cf07c7eba5706b62393ff59be
@@ -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
@@ -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

@@ -65,7 +65,6 @@
#pragma GCC diagnostic pop
#endif


namespace logger = madara::logger;
namespace knowledge = madara::knowledge;
namespace containers = knowledge::containers;
@@ -80,22 +79,28 @@ 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 = "";

//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<RosIntrospection::SubstitutionRule> 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;
@@ -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<mk::CapnObject<Joy> >("Joy");
mk::Any::register_type<mk::CapnObject<LaserScan> >("LaserScan");
mk::Any::register_type<mk::CapnObject<MagneticField> >("MagneticField");
mk::Any::register_type<mk::CapnObject<PointCloud> >("PointCloud");
mk::Any::register_type<mk::CapnObject<PointCloud2> >("PointCloud2");
mk::Any::register_type<mk::CapnObject<PointCloudXYZ> >("PointCloudXYZ");
mk::Any::register_type<mk::CapnObject<PointCloudXYZI> >("PointCloudXYZI");
mk::Any::register_type<mk::CapnObject<PointField> >("PointField");
mk::Any::register_type<mk::CapnObject<Temperature> >("Temperature");
mk::Any::register_type<mk::CapnObject<TimeReference> >("TimeReference");
mk::Any::register_type<mk::CapnObject<RegionOfInterest> >("RegionOfInterest");
mk::Any::register_type<mk::CapnObject<Point> >("Point");
mk::Any::register_type<mk::CapnObject<PointXYZ> >("PointXYZ");
mk::Any::register_type<mk::CapnObject<PointXYZI> >("PointXYZI");
mk::Any::register_type<mk::CapnObject<MapMetaData> >("MapMetaData");
mk::Any::register_type<mk::CapnObject<Pose> >("Pose");
mk::Any::register_type<mk::CapnObject<Quaternion> >("Quaternion");

This file was deleted.

This file was deleted.

@@ -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);

}
@@ -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);

}

@@ -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;

}
@@ -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;

}

0 comments on commit 4d4de85

Please sign in to comment.
You can’t perform that action at this time.