Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[4666] pkg/trunk
|
From: <jl...@us...> - 2008-09-25 02:16:54
|
Revision: 4666
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4666&view=rev
Author: jleibs
Date: 2008-09-25 02:16:41 +0000 (Thu, 25 Sep 2008)
Log Message:
-----------
Logging now stores data types of logged data in a new, slightly different format.
Backwards compatibility has been maintained. The flexibility of LogPlayback has
also been improved with more template magic and the ability to use callbacks which
are class members.
Modified Paths:
--------------
pkg/trunk/drivers/imu/imu_node/manifest.xml
pkg/trunk/nav/laser_pose_interpolator/gen_carmen_laser.cpp
pkg/trunk/util/logging/include/logging/LogPlayer.h
pkg/trunk/util/logging/include/logging/LogRecorder.h
pkg/trunk/util/logsetta/CMakeLists.txt
pkg/trunk/util/logsetta/base_actual/CMakeLists.txt
pkg/trunk/util/logsetta/base_actual/base_actual_extract.cpp
pkg/trunk/util/logsetta/carmen/genlog_carmen/genlog_carmen.cpp
pkg/trunk/util/logsetta/imu/imu_extract.cpp
pkg/trunk/util/logsetta/localize_extract/localize_extract.cpp
pkg/trunk/util/logsetta/manifest.xml
pkg/trunk/util/logsetta/odom/odom_extract.cpp
pkg/trunk/util/megamaid/src/central_vac/central_vac.cpp
pkg/trunk/util/megamaid/src/dustbuster/dustbuster.cpp
pkg/trunk/util/megamaid/src/playback/playback.cpp
pkg/trunk/util/megamaid/src/stepback/stepback.cpp
pkg/trunk/util/megamaid/src/vacuum/vacuum.cpp
pkg/trunk/vision/calib_converter/src/calib_converter.cpp
Added Paths:
-----------
pkg/trunk/util/logging/CMakeLists.txt
pkg/trunk/util/logging/FORMATS
pkg/trunk/util/logging/Makefile
pkg/trunk/util/logging/include/logging/LogFunctor.h
pkg/trunk/util/logging/src/
pkg/trunk/util/logging/src/checklog/
pkg/trunk/util/logging/src/checklog/checklog.cpp
pkg/trunk/util/logging/src/demo/
pkg/trunk/util/logging/src/demo/demo.cpp
Modified: pkg/trunk/drivers/imu/imu_node/manifest.xml
===================================================================
--- pkg/trunk/drivers/imu/imu_node/manifest.xml 2008-09-25 01:29:12 UTC (rev 4665)
+++ pkg/trunk/drivers/imu/imu_node/manifest.xml 2008-09-25 02:16:41 UTC (rev 4666)
@@ -8,4 +8,7 @@
<depend package='std_msgs'/>
<depend package='3dmgx2_driver'/>
<depend package='self_test'/>
+ <export>
+ <cpp cflags="-I${prefix}/msg/cpp"/>
+ </export>
</package>
Modified: pkg/trunk/nav/laser_pose_interpolator/gen_carmen_laser.cpp
===================================================================
--- pkg/trunk/nav/laser_pose_interpolator/gen_carmen_laser.cpp 2008-09-25 01:29:12 UTC (rev 4665)
+++ pkg/trunk/nav/laser_pose_interpolator/gen_carmen_laser.cpp 2008-09-25 02:16:41 UTC (rev 4666)
@@ -42,11 +42,8 @@
const float LASER_POSITION = 0.05;
-void scan_callback(string name, ros::msg* m, ros::Time t, void* n)
+void scan_callback(string name, laser_pose_interpolator::PoseLaserScan* scan, ros::Time t, void* n)
{
-
- laser_pose_interpolator::PoseLaserScan* scan = (laser_pose_interpolator::PoseLaserScan*)(m);
-
double rel_time = t.to_double();
//const double fov = fabs(scan->scan.angle_max - scan->scan.angle_min);
@@ -99,16 +96,8 @@
player.open(files, ros::Time(0));
- int count;
+ player.addHandler<laser_pose_interpolator::PoseLaserScan>(string("pose_scan"), &scan_callback, NULL);
- count = player.addHandler<laser_pose_interpolator::PoseLaserScan>(string("pose_scan"), &scan_callback, NULL, true);
-
- if (count != 1)
- {
- printf("Found %d '/pose_scan' topics when expecting 1\n", count);
- return 1;
- }
-
clog = fopen("carmen.txt", "w");
while(player.nextMsg()) {}
Added: pkg/trunk/util/logging/CMakeLists.txt
===================================================================
--- pkg/trunk/util/logging/CMakeLists.txt (rev 0)
+++ pkg/trunk/util/logging/CMakeLists.txt 2008-09-25 02:16:41 UTC (rev 4666)
@@ -0,0 +1,8 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(logging)
+
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+
+rospack_add_executable(checklog src/checklog/checklog.cpp)
+rospack_add_executable(demo src/demo/demo.cpp)
\ No newline at end of file
Added: pkg/trunk/util/logging/FORMATS
===================================================================
--- pkg/trunk/util/logging/FORMATS (rev 0)
+++ pkg/trunk/util/logging/FORMATS 2008-09-25 02:16:41 UTC (rev 4666)
@@ -0,0 +1,40 @@
+Version 0:
+-------------
+topic_name
+md5sum
+datatype
+<repeating N>
+ time.sec
+ time.nsec
+ length
+ MSGDATA
+-------------
+
+Version 1.0:
+-------------
+#ROSLOG V1.0
+quantity
+<repeating quantity>
+ topic_name
+ md5sum
+ datatype
+<repeating N>
+ topic_name
+ time.sec
+ time.nsec
+ length
+ MSGDATA
+-------------
+
+Version 1.1:
+-------------
+#ROSLOG V1.1
+<repeating N>
+ topic_name
+ md5sum
+ datatype
+ time.sec
+ time.nsec
+ length
+ MSGDATA
+-------------
Added: pkg/trunk/util/logging/Makefile
===================================================================
--- pkg/trunk/util/logging/Makefile (rev 0)
+++ pkg/trunk/util/logging/Makefile 2008-09-25 02:16:41 UTC (rev 4666)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/util/logging/include/logging/LogFunctor.h
===================================================================
--- pkg/trunk/util/logging/include/logging/LogFunctor.h (rev 0)
+++ pkg/trunk/util/logging/include/logging/LogFunctor.h 2008-09-25 02:16:41 UTC (rev 4666)
@@ -0,0 +1,103 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef LOGFUNCTOR_H
+#define LOGFUNCTOR_H
+
+#include "ros/time.h"
+#include <string>
+
+class AbstractLogFunctor
+{
+public:
+ virtual void call(std::string, ros::msg*, ros::Time) = 0;
+ virtual ros::msg* allocateMsg() = 0;
+ virtual ~AbstractLogFunctor() {}
+};
+
+class EmptyObject
+{
+
+};
+
+template<class M, class T = EmptyObject>
+class LogFunctor : public AbstractLogFunctor
+{
+public:
+ LogFunctor(void (*fp)(std::string, M*, ros::Time, void*), void* user_data)
+ : inflate_(true), obj_(NULL), fp_(NULL), fp_typed_(fp), fp_obj_(NULL), fp_typed_obj_(NULL), user_data_(user_data) { }
+
+ LogFunctor(void (*fp)(std::string, ros::msg*, ros::Time, void*), void* user_data, bool inflate)
+ : inflate_(inflate), obj_(NULL), fp_(fp), fp_typed_(NULL), fp_obj_(NULL), fp_typed_obj_(NULL), user_data_(user_data) { }
+
+ LogFunctor(T* obj, void (T::*fp)(std::string, M*, ros::Time, void*), void* user_data)
+ : inflate_(true), obj_(obj), fp_(NULL), fp_typed_(NULL), fp_obj_(NULL), fp_typed_obj_(fp), user_data_(user_data) { }
+
+ LogFunctor(T* obj, void (T::*fp)(std::string, ros::msg*, ros::Time, void*), void* user_data, bool inflate)
+ : inflate_(inflate), obj_(obj), fp_(NULL), fp_typed_(NULL), fp_obj_(fp), fp_typed_obj_(NULL), user_data_(user_data) { }
+
+ virtual void call(std::string topic_name, ros::msg* msg, ros::Time time)
+ {
+ if (obj_)
+ {
+ if (fp_obj_)
+ (*obj_.*fp_obj_)(topic_name, msg, time, user_data_);
+ else if(fp_typed_obj_)
+ (*obj_.*fp_typed_obj_)(topic_name, (M*)msg, time, user_data_);
+ } else {
+ if (fp_)
+ (*fp_)(topic_name, msg, time, user_data_);
+ else if (fp_typed_)
+ (*fp_typed_)(topic_name, (M*)msg, time, user_data_);
+ }
+ }
+
+ virtual ros::msg* allocateMsg()
+ {
+ if (fp_typed_ || fp_typed_obj_)
+ return new M;
+ else
+ return NULL;
+ }
+private:
+ bool inflate_;
+ T* obj_;
+ void (*fp_)(std::string, ros::msg*, ros::Time, void*);
+ void (*fp_typed_)(std::string, M*, ros::Time, void*);
+ void (T::*fp_obj_)(std::string, ros::msg*, ros::Time, void*);
+ void (T::*fp_typed_obj_)(std::string, M*, ros::Time, void*);
+ void *user_data_;
+};
+
+#endif
Modified: pkg/trunk/util/logging/include/logging/LogPlayer.h
===================================================================
--- pkg/trunk/util/logging/include/logging/LogPlayer.h 2008-09-25 01:29:12 UTC (rev 4665)
+++ pkg/trunk/util/logging/include/logging/LogPlayer.h 2008-09-25 02:16:41 UTC (rev 4666)
@@ -38,30 +38,39 @@
#include "ros/node.h"
#include "ros/time.h"
#include "logging/AnyMsg.h"
+#include "logging/LogFunctor.h"
#include <fstream>
#include <sstream>
+#include <cstdio>
class LogPlayer
{
-
+ struct FilteredLogFunctor
+ {
+ std::string topic_name;
+ std::string md5sum;
+ std::string datatype;
+ bool inflate;
+ AbstractLogFunctor* f;
+ };
+
+
class LogHelper : public ros::msg
{
- typedef std::pair<void (*)(std::string, ros::msg*, ros::Time, void*), void*> Callback;
+ LogPlayer* log_player_;
std::string topic_name_;
+ std::string md5sum_;
std::string datatype_;
- std::string md5sum_;
- std::vector<Callback> callbacks_;
-
ros::msg* msg_;
uint8_t* next_msg_;
uint32_t next_msg_size_;
public:
- LogHelper(std::string topic_name, std::string datatype, std::string md5sum)
- : topic_name_(topic_name), datatype_(datatype), md5sum_(md5sum),
+ LogHelper(LogPlayer* log_player, std::string topic_name, std::string md5sum, std::string datatype)
+ : log_player_(log_player), topic_name_(topic_name), md5sum_(md5sum), datatype_(datatype),
msg_(NULL), next_msg_(NULL), next_msg_size_(0) {}
virtual ~LogHelper()
@@ -70,38 +79,55 @@
delete msg_;
}
- void addHandler(ros::msg* msg, void (*fp)(std::string, ros::msg*, ros::Time, void*), void* ptr)
+ void callHandlers()
{
- if (msg)
- {
- if (msg_)
- delete msg_;
- msg_ = msg;
- }
- callbacks_.push_back(Callback(fp, ptr));
- }
+ next_msg_ = log_player_->next_msg_;
+ next_msg_size_ = log_player_->next_msg_size_;
- void callHandlers(uint8_t* next_msg, uint32_t next_msg_size, ros::Time time)
- {
- next_msg_ = next_msg;
- next_msg_size_ = next_msg_size;
-
__serialized_length = next_msg_size_;
- ros::msg* msg = this;
-
if (msg_)
{
msg_->__serialized_length = next_msg_size_;
msg_->deserialize(next_msg_);
- msg = msg_;
}
- for (std::vector<Callback>::iterator cb_it = callbacks_.begin();
- cb_it != callbacks_.end();
- cb_it++)
- (*(cb_it->first))(topic_name_, msg, time, (cb_it->second));
+ for (std::vector<FilteredLogFunctor>::iterator flf_it = log_player_->callbacks_.begin();
+ flf_it != log_player_->callbacks_.end();
+ flf_it++)
+ {
+
+ if (topic_name_ == flf_it->topic_name ||
+ flf_it->topic_name == std::string("*"))
+ {
+
+ if (flf_it->md5sum != md5sum_ &&
+ md5sum_ != std::string("*"))
+ break;
+
+ if (flf_it->datatype != datatype_ &&
+ flf_it->datatype != std::string("*") &&
+ datatype_ != std::string("*"))
+ break;
+
+ if (flf_it->inflate && msg_ == NULL)
+ {
+ msg_ = flf_it->f->allocateMsg();
+
+ if (msg_)
+ {
+ msg_->__serialized_length = next_msg_size_;
+ msg_->deserialize(next_msg_);
+ }
+ }
+
+ if (flf_it->inflate)
+ flf_it->f->call(topic_name_, msg_, log_player_->next_msg_time_);
+ else
+ flf_it->f->call(topic_name_, this, log_player_->next_msg_time_);
+ }
+ }
}
std::string get_topic_name() {return topic_name_;}
@@ -124,25 +150,30 @@
std::ifstream log_file_;
- bool old_format_;
+ int version_;
ros::Time start_time_;
std::map<std::string, LogHelper*> topics_;
std::string next_msg_name_;
- uint8_t *next_msg_;
- uint32_t next_msg_size_, next_msg_alloc_size_;
+
ros::Time next_msg_time_;
bool done_;
public:
- LogPlayer() : old_format_(false), next_msg_(NULL), next_msg_size_(0), next_msg_alloc_size_(0), done_(false) {}
+ LogPlayer() : version_(0), done_(false), next_msg_(NULL), next_msg_size_(0), next_msg_alloc_size_(0) {}
virtual ~LogPlayer()
{
+ for (std::vector<FilteredLogFunctor>::iterator flf_it = callbacks_.begin();
+ flf_it != callbacks_.end();
+ flf_it++)
+ if (flf_it->f)
+ delete (flf_it->f);
+
close();
}
@@ -175,44 +206,65 @@
return false;
}
- std::string comment_line;
- getline(log_file_, comment_line);
+ int version_major = 0;
+ int version_minor = 0;
- if (comment_line[0] != '#')
- log_file_.seekg(0, std::ios_base::beg);
+ std::string version_line;
+ getline(log_file_, version_line);
- std::string quantity_line;
- getline(log_file_,quantity_line);
+ sscanf(version_line.c_str(), "#ROSLOG V%d.%d", &version_major, &version_minor);
- std::istringstream iss(quantity_line);
+ if (version_major == 0 && version_line[0] == '#')
+ {
+ version_major = 1;
+ }
- int quantity = -1;
- iss >> quantity;
-
- std::cout << "Reading " << quantity << " topics from " << file_name << std::endl;
+ version_ = version_major * 100 + version_minor;
- if (quantity == -1)
+ int quantity = 0;
+
+ if (version_ == 0)
{
- old_format_ = true;
+ std::cout << "Opening ROSLOG version 0" << std::endl;
+
+ log_file_.seekg(0, std::ios_base::beg);
+
quantity = 1;
- log_file_.seekg(-(int)(quantity_line.size() + 1), std::ios_base::cur);
+
}
+ else if (version_ == 100)
+ {
+ std::cout << "Opening ROSLOG version 1.0" << std::endl;
- std::string topic_name;
- std::string md5sum;
- std::string datatype;
+ std::string quantity_line;
+ getline(log_file_,quantity_line);
+ sscanf(quantity_line.c_str(), "%d", &quantity);
+ }
+ else if (version_ == 101)
+ {
+ std::cout << "Opening ROSLOG version 1.1" << std::endl;
+ }
- for (int i = 0; i < quantity; i++)
+ if (version_ == 0 || version_ == 100)
{
- getline(log_file_,topic_name);
- getline(log_file_,md5sum);
- getline(log_file_,datatype);
+ // std::cout << "Reading " << quantity << " topics from " << file_name << std::endl;
- std::cout << " (" << i << "): " << topic_name << ", " << md5sum << ", " << datatype << std::endl;
+ std::string topic_name;
+ std::string md5sum;
+ std::string datatype;
- LogHelper* l = new LogHelper(topic_name, md5sum, datatype);
-
- topics_[topic_name] = l;
+ for (int i = 0; i < quantity; i++)
+ {
+ getline(log_file_,topic_name);
+ getline(log_file_,md5sum);
+ getline(log_file_,datatype);
+
+ // std::cout << " (" << i << "): " << topic_name << ", " << md5sum << ", " << datatype << std::endl;
+
+ LogHelper* l = new LogHelper(this, topic_name, md5sum, datatype);
+
+ topics_[topic_name] = l;
+ }
}
readNextMsg();
@@ -220,53 +272,62 @@
return true;
}
+ template <class M>
+ void addHandler(std::string topic_name, void (*fp)(std::string, ros::msg*, ros::Time, void*), void* ptr, bool inflate)
+ {
+ FilteredLogFunctor flf;
- std::vector<std::string> getNames() {
- std::vector<std::string> names;
- for (std::map<std::string, LogHelper*>::iterator topic_it = topics_.begin();
- topic_it != topics_.end();
- topic_it++)
- names.push_back(topic_it->first);
+ flf.topic_name = topic_name;
+ flf.md5sum = M::__s_get_md5sum();
+ flf.datatype = M::__s_get_datatype();
+ flf.inflate = inflate;
+ flf.f = new LogFunctor<M>(fp, ptr, inflate);
- return names;
+ callbacks_.push_back(flf);
}
template <class M>
- int addHandler(std::string topic_name, void (*fp)(std::string, ros::msg*, ros::Time, void*), void* ptr, bool inflate)
+ void addHandler(std::string topic_name, void (*fp)(std::string, M*, ros::Time, void*), void* ptr)
{
+ FilteredLogFunctor flf;
- int count = 0;
+ flf.topic_name = topic_name;
+ flf.md5sum = M::__s_get_md5sum();
+ flf.datatype = M::__s_get_datatype();
+ flf.inflate = true;
+ flf.f = new LogFunctor<M>(fp, ptr);
- for (std::map<std::string, LogHelper*>::iterator topic_it = topics_.begin();
- topic_it != topics_.end();
- topic_it++)
- {
- if (topic_name == std::string("*") ||
- topic_name == (topic_it->second)->get_topic_name())
- {
+ callbacks_.push_back(flf);
+ }
- if (M::__s_get_md5sum() != (topic_it->second)->__get_md5sum() &&
- (topic_it->second)->__get_md5sum() != std::string("*"))
- break;
+ template <class M, class T>
+ void addHandler(std::string topic_name, void (T::*fp)(std::string, ros::msg*, ros::Time, void*), T* obj, void* ptr, bool inflate)
+ {
+ FilteredLogFunctor flf;
- if (M::__s_get_datatype() != (topic_it->second)->__get_datatype() &&
- M::__s_get_datatype() != std::string("*") && (topic_it->second)->__get_datatype() != std::string("*"))
- break;
-
- M* msg = NULL;
+ flf.topic_name = topic_name;
+ flf.md5sum = M::__s_get_md5sum();
+ flf.datatype = M::__s_get_datatype();
+ flf.inflate = inflate;
+ flf.f = new LogFunctor<M, T>(obj, fp, ptr, inflate);
- if (inflate)
- msg = new M;
-
- (topic_it->second)->addHandler(msg, fp, ptr);
- count++;
-
- }
- }
- return count;
+ callbacks_.push_back(flf);
}
+ template <class M, class T>
+ void addHandler(std::string topic_name, void (T::*fp)(std::string, M*, ros::Time, void*), T* obj, void* ptr)
+ {
+ FilteredLogFunctor flf;
+ flf.topic_name = topic_name;
+ flf.md5sum = M::__s_get_md5sum();
+ flf.datatype = M::__s_get_datatype();
+ flf.inflate = true;
+ flf.f = new LogFunctor<M, T>(obj, fp, ptr);
+
+ callbacks_.push_back(flf);
+ }
+
ros::Time get_next_msg_time()
{
if (!done_)
@@ -279,17 +340,24 @@
{
if (done_)
return false;
-
+
if (topics_.find(next_msg_name_) != topics_.end())
- topics_[next_msg_name_]->callHandlers(next_msg_, next_msg_size_, next_msg_time_);
-
+ {
+ topics_[next_msg_name_]->callHandlers();
+ }
+
readNextMsg();
-
+
return true;
}
-
+
protected:
+ uint8_t *next_msg_;
+ uint32_t next_msg_size_, next_msg_alloc_size_;
+
+ std::vector<FilteredLogFunctor> callbacks_;
+
bool readNextMsg()
{
if (!log_file_.good())
@@ -297,28 +365,51 @@
done_ = true;
return false;
}
+
+ if (version_ <= 100)
+ {
+ if (version_ == 0)
+ next_msg_name_ = (topics_.begin())->first;
+ else
+ getline(log_file_, next_msg_name_);
+
+ ros::Duration next_msg_dur;
- if (old_format_)
- {
- next_msg_name_ = (topics_.begin())->first;
+
} else {
- getline(log_file_, next_msg_name_);
+
+ std::string topic_name;
+ std::string md5sum;
+ std::string datatype;
+
+ getline(log_file_,topic_name);
+ getline(log_file_,md5sum);
+ getline(log_file_,datatype);
+
+ next_msg_name_ = topic_name;
+
+ if (topics_.find(topic_name) == topics_.end())
+ {
+ LogHelper* l = new LogHelper(this, topic_name, md5sum, datatype);
+ topics_[topic_name] = l;
+ }
+
}
ros::Duration next_msg_dur;
-
+
log_file_.read((char*)&next_msg_dur.sec, 4);
log_file_.read((char*)&next_msg_dur.nsec, 4);
log_file_.read((char*)&next_msg_size_, 4);
-
+
next_msg_time_ = start_time_ + next_msg_dur;
-
+
if (log_file_.eof())
{
done_ = true;
return false;
}
-
+
if (next_msg_size_ > next_msg_alloc_size_)
{
if (next_msg_)
@@ -326,7 +417,7 @@
next_msg_alloc_size_ = next_msg_size_ * 2;
next_msg_ = new uint8_t[next_msg_alloc_size_];
}
-
+
log_file_.read((char*)next_msg_, next_msg_size_);
if (log_file_.eof())
@@ -334,10 +425,10 @@
done_ = true;
return false;
}
+
+ return true;
- return true;
}
-
};
class MultiLogPlayer
@@ -379,25 +470,6 @@
return true;
}
- std::vector<std::string> getNames() {
-
- std::vector<std::string> names;
-
- for (std::vector<LogPlayer*>::iterator player_it = players_.begin();
- player_it != players_.end();
- player_it++)
- {
- std::vector<std::string> n = (*player_it)->getNames();
-
- for (std::vector<std::string>::iterator topic_it = n.begin();
- topic_it != n.end();
- topic_it++)
- names.push_back(*topic_it);
- }
-
- return names;
- }
-
bool nextMsg()
{
LogPlayer* next_player = 0;
@@ -433,17 +505,49 @@
}
template <class M>
- int addHandler(std::string topic_name, void (*fp)(std::string, ros::msg*, ros::Time, void*), void* ptr, bool inflate)
+ void addHandler(std::string topic_name, void (*fp)(std::string, ros::msg*, ros::Time, void*), void* ptr, bool inflate)
{
- int count = 0;
for (std::vector<LogPlayer*>::iterator player_it = players_.begin();
player_it != players_.end();
player_it++)
{
- count += (*player_it)->addHandler<M>(topic_name, fp, ptr, inflate);
+ (*player_it)->addHandler<M>(topic_name, fp, ptr, inflate);
}
- return count;
}
+
+ template <class M>
+ void addHandler(std::string topic_name, void (*fp)(std::string, M*, ros::Time, void*), void* ptr)
+ {
+ for (std::vector<LogPlayer*>::iterator player_it = players_.begin();
+ player_it != players_.end();
+ player_it++)
+ {
+ (*player_it)->addHandler<M>(topic_name, fp, ptr);
+ }
+ }
+
+ template <class M, class T>
+ void addHandler(std::string topic_name, void (T::*fp)(std::string, ros::msg*, ros::Time, void*), T* obj, void* ptr, bool inflate)
+ {
+ for (std::vector<LogPlayer*>::iterator player_it = players_.begin();
+ player_it != players_.end();
+ player_it++)
+ {
+ (*player_it)->addHandler<M>(topic_name, fp, obj, ptr, inflate);
+ }
+ }
+
+ template <class M, class T>
+ void addHandler(std::string topic_name, void (T::*fp)(std::string, M*, ros::Time, void*), T* obj, void* ptr)
+ {
+ for (std::vector<LogPlayer*>::iterator player_it = players_.begin();
+ player_it != players_.end();
+ player_it++)
+ {
+ (*player_it)->addHandler<M>(topic_name, fp, obj, ptr);
+ }
+ }
};
#endif
+
Modified: pkg/trunk/util/logging/include/logging/LogRecorder.h
===================================================================
--- pkg/trunk/util/logging/include/logging/LogRecorder.h 2008-09-25 01:29:12 UTC (rev 4665)
+++ pkg/trunk/util/logging/include/logging/LogRecorder.h 2008-09-25 02:16:41 UTC (rev 4666)
@@ -37,7 +37,7 @@
#include "ros/time.h"
#include "rosthread/mutex.h"
-#include "logging/AnyMsg.h"
+#include "logging/LogFunctor.h"
#include <fstream>
#include <iostream>
@@ -47,34 +47,35 @@
class LogRecorder
{
+ typedef std::vector<std::pair<std::string, std::string> > TopicList;
+
class LogHelper : public ros::msg
{
- typedef std::pair<void (*)(std::string, ros::msg*, void*), void*> Callback;
-
LogRecorder* log_recorder_;
std::string topic_name_;
std::string datatype_;
std::string md5sum_;
- Callback* callback_;
+ AbstractLogFunctor* callback_;
ros::msg* msg_;
- unsigned int max_count_;
- unsigned int count_;
+ unsigned int max_queue_;
+ bool first_;
public:
- LogHelper(LogRecorder* log_recorder, std::string topic_name, std::string datatype, std::string md5sum, unsigned int max_count = 0)
+ LogHelper(LogRecorder* log_recorder, std::string topic_name, std::string datatype, std::string md5sum, unsigned int max_queue)
: log_recorder_(log_recorder),
topic_name_(topic_name), datatype_(datatype), md5sum_(md5sum),
callback_(NULL),
msg_(NULL),
- max_count_(max_count),
- count_(0) {}
+ max_queue_(max_queue),
+ first_(true)
+ {}
virtual ~LogHelper()
{
@@ -85,36 +86,29 @@
delete callback_;
}
-
- void addHandler(ros::msg* msg, void (*fp)(std::string, ros::msg*, void*), void* ptr)
+ void addHandler(AbstractLogFunctor* callback)
{
- if (msg)
- {
- if (msg_)
- delete msg_;
- msg_ = msg;
- }
+ if (msg_ == NULL)
+ msg_ = callback->allocateMsg();
+ else
+ msg_ = this;
- callback_ = new Callback(fp, ptr);
+ callback_ = callback;
}
void callHandler()
{
- if (count_++ < max_count_ || max_count_ == 0)
+ if (callback_)
{
- ros::msg* msg = this;
-
- if (msg_)
- msg = msg_;
-
- if (callback_)
- (*(callback_->first))(topic_name_, msg, (callback_->second));
+ ros::Time now = ros::Time::now();
+ callback_->call(topic_name_, msg_, now);
}
}
-
std::string get_topic_name() {return topic_name_;}
+ int get_max_queue() {return max_queue_;}
+
virtual const std::string __get_datatype() const { return datatype_; }
virtual const std::string __get_md5sum() const { return md5sum_; }
@@ -125,24 +119,39 @@
virtual uint8_t *deserialize(uint8_t *read_ptr)
{
+ if (first_)
+ {
+ std::string mapped_topic_name = log_recorder_->node_->map_name(topic_name_);
+
+ if (datatype_ == "*")
+ {
+ TopicList topics;
+
+ log_recorder_->node_->get_published_topics(&topics);
+
+ for (TopicList::iterator i = topics.begin(); i != topics.end(); i++)
+ if (i->first == mapped_topic_name)
+ {
+ datatype_ = i->second;
+ break;
+ }
+ }
+ }
+
uint8_t* sz = read_ptr + __serialized_length;
- if (count_ < max_count_ || max_count_ == 0)
- {
- sz = log_recorder_->log(get_topic_name(), read_ptr, __serialized_length);
- if (msg_)
- {
- msg_->__serialized_length = __serialized_length;
- msg_->deserialize(read_ptr);
- }
+ sz = log_recorder_->log(get_topic_name(), __get_datatype(), __get_md5sum(), read_ptr, __serialized_length);
+
+ if (msg_)
+ {
+ msg_->__serialized_length = __serialized_length;
+ msg_->deserialize(read_ptr);
}
return sz;
}
};
- ros::node* node_;
-
std::ofstream log_file_;
ros::thread::mutex log_mutex_;
@@ -151,9 +160,11 @@
std::vector<LogHelper*> topics_;
+ bool started_;
+
public:
- LogRecorder(ros::node* node) : node_(node) {}
+ LogRecorder(ros::node* node) : started_(false), node_(node) {}
virtual ~LogRecorder()
{
@@ -179,62 +190,93 @@
return false;
}
- log_file_ << "#ROSLOG V1.0" << std::endl << std::setw(4) << std::setfill('0') << 0 << std::endl;
+ log_file_ << "#ROSLOG V1.1" << std::endl;
return true;
}
+ template <class M>
+ LogHelper* addTopic_(std::string topic_name, int max_queue)
+ {
+ if (!started_)
+ {
+ std::string datatype = M::__s_get_datatype();
+ std::string md5sum = M::__s_get_md5sum();
+
+ LogHelper* l = new LogHelper(this, topic_name, datatype, md5sum, max_queue);
+
+ topics_.push_back(l);
+
+ return l;
+
+ } else {
+ std::cerr << "Tried to add topic \"" << topic_name << "\" after logging started." << std::endl;
+ return NULL;
+ }
+ }
+
template <class M>
- void addTopic(std::string topic_name, void (*fp)(std::string, ros::msg*, void*) = NULL, void* ptr = NULL, bool inflate = false, unsigned int max_count = 0)
+ void addTopic(std::string topic_name, int max_queue)
{
-
- LogHelper* l = new LogHelper(this, topic_name, M::__s_get_datatype(), M::__s_get_md5sum(), max_count);
- topics_.push_back(l);
+ addTopic_<M>(topic_name, max_queue);
+ }
- log_mutex_.lock();
+ template <class M>
+ void addTopic(std::string topic_name, int max_queue, void (*fp)(std::string, ros::msg*, ros::Time, void*), void* ptr = NULL, bool inflate = false)
+ {
+ LogHelper* l = addTopic_<M>(topic_name, max_queue);
+
+ if (!l)
+ return;
- log_file_.seekp(0, std::ios_base::beg);
- log_file_ << "#ROSLOG V1.0" << std::endl << std::setw(4) << std::setfill('0') << topics_.size() << std::endl;
- log_file_.seekp(0, std::ios_base::end);
+ if (fp != NULL)
+ {
+ AbstractLogFunctor* lf = new LogFunctor<M>(fp, ptr, inflate);
+ l->addHandler(lf);
+ }
+ }
- log_file_ << topic_name << std::endl;
- log_file_ << l->__get_datatype() << std::endl;
- log_file_ << l->__get_md5sum() << std::endl;
+ template <class M>
+ void addTopic(std::string topic_name, int max_queue, void (*fp)(std::string, M*, ros::Time, void*), void* ptr = NULL)
+ {
+ LogHelper* l = addTopic_<M>(topic_name, max_queue);
- log_mutex_.unlock();
+ if (!l)
+ return;
if (fp != NULL)
{
- M* msg = NULL;
-
- if (inflate)
- msg = new M;
-
- l->addHandler(msg, fp, ptr);
+ AbstractLogFunctor* lf = new LogFunctor<M>(fp, ptr);
+ l->addHandler(lf);
}
-
}
void start() {
for (std::vector<LogHelper*>::iterator topic_it = topics_.begin();
topic_it != topics_.end();
topic_it++)
- node_->subscribe((*topic_it)->get_topic_name(), *(*topic_it), &LogRecorder::dummyCb, this, (*topic_it), 100);
+ node_->subscribe((*topic_it)->get_topic_name(), *(*topic_it), &LogRecorder::loggerCb, this, (*topic_it), (*topic_it)->get_max_queue());
}
- void dummyCb(void* log_helper)
+ void loggerCb(void* log_helper)
{
((LogHelper*)(log_helper))->callHandler();
}
+
protected:
- uint8_t* log(std::string topic_name, uint8_t *read_ptr, uint32_t length)
+
+ ros::node* node_;
+
+ uint8_t* log(std::string topic_name, std::string datatype, std::string md5sum, uint8_t *read_ptr, uint32_t length)
{
ros::Duration elapsed = ros::Time::now() - start_time_;
log_mutex_.lock();
log_file_ << topic_name << std::endl;
+ log_file_ << md5sum << std::endl;
+ log_file_ << datatype << std::endl;
log_file_.write((char*)&elapsed.sec, 4);
log_file_.write((char*)&elapsed.nsec, 4);
Added: pkg/trunk/util/logging/src/checklog/checklog.cpp
===================================================================
--- pkg/trunk/util/logging/src/checklog/checklog.cpp (rev 0)
+++ pkg/trunk/util/logging/src/checklog/checklog.cpp 2008-09-25 02:16:41 UTC (rev 4666)
@@ -0,0 +1,88 @@
+///////////////////////////////////////////////////////////////////////////////
+// The megamaid package provides logging and playback
+//
+// Copyright (C) 2008, Morgan Quigley
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+#include "ros/node.h"
+#include <string>
+#include <map>
+
+#include "logging/LogPlayer.h"
+#include "logging/AnyMsg.h"
+
+using namespace std;
+
+struct LogContent
+{
+ string datatype;
+ int count;
+ LogContent(string d) : datatype(d), count(1) {}
+};
+
+map<string, LogContent> g_content;
+
+void checkFile(string name, ros::msg* m, ros::Time t, void* n)
+{
+ map<string, LogContent>::iterator i = g_content.find(name);
+
+ if (i == g_content.end())
+ {
+ g_content.insert(pair<string, LogContent>(name, LogContent(m->__get_datatype())));
+ } else {
+ i->second.count++;
+ }
+}
+
+int main(int argc, char **argv)
+{
+ if (argc != 2)
+ {
+ printf("\nusage:\n");
+ printf("checklog BAG\n");
+ return 1;
+ }
+
+ LogPlayer player;
+
+ if (player.open(string(argv[1]), ros::Time(0)))
+ {
+ player.addHandler<AnyMsg>(string("*"), &checkFile, NULL, false);
+ }
+
+ while(player.nextMsg())
+ {
+ }
+
+ for (map<string, LogContent>::iterator i = g_content.begin();
+ i != g_content.end();
+ i++)
+ {
+ printf(" Name: %s Datatype: %s Count: %d\n", i->first.c_str(), i->second.datatype.c_str(), i->second.count);
+ }
+
+ return 0;
+}
Added: pkg/trunk/util/logging/src/demo/demo.cpp
===================================================================
--- pkg/trunk/util/logging/src/demo/demo.cpp (rev 0)
+++ pkg/trunk/util/logging/src/demo/demo.cpp 2008-09-25 02:16:41 UTC (rev 4666)
@@ -0,0 +1,96 @@
+///////////////////////////////////////////////////////////////////////////////
+// The megamaid package provides logging and playback
+//
+// Copyright (C) 2008, Morgan Quigley
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+#include "ros/node.h"
+#include <string>
+#include <map>
+
+#include "logging/LogPlayer.h"
+#include "logging/AnyMsg.h"
+#include "std_msgs/String.h"
+
+using namespace std;
+
+void all_handler(string name, ros::msg* m, ros::Time t, void* n)
+{
+ int* counter = (int*)(n);
+
+ cout << "all_handler saw a message named " << name << " with datatype: " << m->__get_datatype() << " Count at: " << (*counter)++ << endl;
+}
+
+void string_handler(string name, std_msgs::String* str, ros::Time t, void* n)
+{
+ cout << "string_handler saw a message named " << name << " with datatype: " << str->__get_datatype() << " and value: " << str->data << endl;
+}
+
+class LogHandler
+{
+public:
+ int counter;
+
+ LogHandler() : counter(0) {}
+
+ void all_handler(string name, ros::msg* m, ros::Time t, void* n)
+ {
+ cout << "LogHandler::all_handler saw a message named " << name << " with datatype: " << m->__get_datatype() << " Count at: " << counter++ << endl;
+ }
+
+ void string_handler(string name, std_msgs::String* str, ros::Time t, void* n)
+ {
+ cout << "LogHandler::string_handler saw a message named " << name << " with datatype: " << str->__get_datatype() << " and value: " << str->data << endl;
+ }
+};
+
+int main(int argc, char **argv)
+{
+ if (argc != 2)
+ {
+ cout << endl << "usage:" << endl << " demo BAG" << endl;
+ return 1;
+ }
+
+ LogPlayer player;
+ LogHandler handler;
+
+ int counter = 0;
+
+ if (player.open(string(argv[1]), ros::Time(0)))
+ {
+ player.addHandler<AnyMsg>(string("*"), &all_handler, &counter, false);
+ player.addHandler<std_msgs::String>(string("chatter"), &string_handler, NULL);
+ player.addHandler<AnyMsg>(string("babble"), &LogHandler::all_handler, &handler, &counter, false);
+ player.addHandler<std_msgs::String>(string("*"), &LogHandler::string_handler, &handler, NULL);
+ }
+
+ while(player.nextMsg())
+ {
+ }
+
+ return 0;
+}
Modified: pkg/trunk/util/logsetta/CMakeLists.txt
===================================================================
--- pkg/trunk/util/logsetta/CMakeLists.txt 2008-09-25 01:29:12 UTC (rev 4665)
+++ pkg/trunk/util/logsetta/CMakeLists.txt 2008-09-25 02:16:41 UTC (rev 4666)
@@ -2,4 +2,4 @@
include(rosbuild)
rospack(logsetta)
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-subdirs(carmen imu odom base_actual localize_extract)
+subdirs(carmen imu odom localize_extract)
Modified: pkg/trunk/util/logsetta/base_actual/CMakeLists.txt
===================================================================
--- pkg/trunk/util/logsetta/base_actual/CMakeLists.txt 2008-09-25 01:29:12 UTC (rev 4665)
+++ pkg/trunk/util/logsetta/base_actual/CMakeLists.txt 2008-09-25 02:16:41 UTC (rev 4666)
@@ -1 +1 @@
-rospack_add_executable(localize_extract localize_extract.cpp)
+rospack_add_executable(base_actual_extract base_actual_extract.cpp)
Modified: pkg/trunk/util/logsetta/base_actual/base_actual_extract.cpp
===================================================================
--- pkg/trunk/util/logsetta/base_actual/base_actual_extract.cpp 2008-09-25 01:29:12 UTC (rev 4665)
+++ pkg/trunk/util/logsetta/base_actual/base_actual_extract.cpp 2008-09-25 02:16:41 UTC (rev 4666)
@@ -32,14 +32,13 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-#include "robot_msgs/BaseActualMsg.h"
+#include "std_msgs/BaseActualMsg.h"
#include <string>
#include "logging/LogPlayer.h"
-void odom_callback(std::string name, ros::msg* m, ros::Time t, void* f)
+void odom_callback(std::string name, std_msgs::BaseActualMsg* baseActual, ros::Time t, void* f)
{
FILE* file = (FILE*)f;
- robot_msgs::BaseActualMsg* baseActual = (robot_msgs::BaseActualMsg*)(m);
fprintf(file, "%.5f ",t.to_double());
@@ -80,14 +79,8 @@
FILE* file = fopen("base_actual.txt", "w");
- count = player.addHandler<robot_msgs::BaseActualMsg>(std::string("baseActual"), &odom_callback, file, true);
+ player.addHandler<robot_msgs::BaseActualMsg>(std::string("baseActual"), &odom_callback, file, true);
- if (count != 1)
- {
- printf("Found %d '/odom' topics when expecting 1", count);
- return 1;
- }
-
while(player.nextMsg()) {}
fclose(file);
Modified: pkg/trunk/util/logsetta/carmen/genlog_carmen/genlog_carmen.cpp
===================================================================
--- pkg/trunk/util/logsetta/carmen/genlog_carmen/genlog_carmen.cpp 2008-09-25 01:29:12 UTC (rev 4665)
+++ pkg/trunk/util/logsetta/carmen/genlog_carmen/genlog_carmen.cpp 2008-09-25 02:16:41 UTC (rev 4666)
@@ -42,11 +42,8 @@
FILE *test_log = NULL;
double prev_x = 0, prev_y = 0, prev_th = 0, dumb_rv = 0, dumb_tv = 0, prev_time;
-void odom_callback(string name, ros::msg* m, ros::Time t, void* n)
+void odom_callback(string name, std_msgs::RobotBase2DOdom* odom, ros::Time t, void* n)
{
-
- std_msgs::RobotBase2DOdom* odom = (std_msgs::RobotBase2DOdom*)(m);
-
double rel_time = t.to_double();
static bool vel_init = false;
@@ -87,11 +84,9 @@
dumb_tv, dumb_rv, rel_time, rel_time);
}
-void scan_callback(string name, ros::msg* m, ros::Time t, void* n)
+void scan_callback(string name, std_msgs::LaserScan* scan, ros::Time t, void* n)
{
- std_msgs::LaserScan* scan = (std_msgs::LaserScan*)(m);
-
double rel_time = t.to_double();
const double fov = fabs(scan->angle_max - scan->angle_min);
@@ -132,24 +127,9 @@
player.open(files, ros::Time(0));
- int count;
+ player.addHandler<std_msgs::RobotBase2DOdom>(string("*"), &odom_callback, NULL);
+ player.addHandler<std_msgs::LaserScan>(string("*"), &scan_callback, NULL);
- count = player.addHandler<std_msgs::RobotBase2DOdom>(string("odom"), &odom_callback, NULL, true);
-
- if (count != 1)
- {
- printf("Found %d '/odom' topics when expecting 1\n", count);
- return 1;
- }
-
- count = player.addHandler<std_msgs::LaserScan>(string("scan"), &scan_callback, NULL, true);
-
- if (count != 1)
- {
- printf("Found %d '/scan' topics when expecting 1\n", count);
- return 1;
- }
-
clog = fopen("carmen.txt", "w");
while(player.nextMsg()) {}
Modified: pkg/trunk/util/logsetta/imu/imu_extract.cpp
===================================================================
--- pkg/trunk/util/logsetta/imu/imu_extract.cpp 2008-09-25 01:29:12 UTC (rev 4665)
+++ pkg/trunk/util/logsetta/imu/imu_extract.cpp 2008-09-25 02:16:41 UTC (rev 4666)
@@ -32,14 +32,13 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-#include "std_msgs/ImuData.h"
+#include "imu_node/ImuData.h"
#include <string>
#include "logging/LogPlayer.h"
-void imu_callback(std::string name, ros::msg* m, ros::Time t, void* f)
+void imu_callback(std::string name, imu_node::ImuData* imu, ros::Time t, void* f)
{
FILE* file = (FILE*)f;
- std_msgs::ImuData* imu = (std_msgs::ImuData*)(m);
fprintf(file, "%.5f %.5f %.5f %.5f %.5f %.5f %.5f %.5f\n",
t.to_double(),
@@ -64,14 +63,8 @@
FILE* file = fopen("imu.txt", "w");
- count = player.addHandler<std_msgs::ImuData>(std::string("/imu_data"), &imu_callback, file, true);
+ player.addHandler<imu_node::ImuData>(std::string("*"), &imu_callback, file);
- if (count != 1)
- {
- printf("Found %d '/imu_data' topics when expecting 1", count);
- return 1;
- }
-
while(player.nextMsg()) {}
fclose(file);
Modified: pkg/trunk/util/logsetta/localize_extract/localize_extract.cpp
===================================================================
--- pkg/trunk/util/logsetta/localize_extract/localize_extract.cpp 2008-09-25 01:29:12 UTC (rev 4665)
+++ pkg/trunk/util/logsetta/localize_extract/localize_extract.cpp 2008-09-25 02:16:41 UTC (rev 4666)
@@ -36,10 +36,9 @@
#include <string>
#include "logging/LogPlayer.h"
-void localize_callback(std::string name, ros::msg* m, ros::Time t, void* f)
+void localize_callback(std::string name, std_msgs::RobotBase2DOdom* bL, ros::Time t, void* f)
{
FILE* file = (FILE*)f;
- std_msgs::RobotBase2DOdom* bL = (std_msgs::RobotBase2DOdom*)(m);
fprintf(file, "%.5f ",t.to_double());
@@ -64,14 +63,8 @@
FILE* file = fopen("localize_actual.txt", "w");
- count = player.addHandler<std_msgs::RobotBase2DOdom>(std::string("localizedPose"), &localize_callback, file, true);
+ player.addHandler<std_msgs::RobotBase2DOdom>(std::string("localizedPose"), &localize_callback, file);
- if (count != 1)
- {
- printf("Found %d '/odom' topics when expecting 1", count);
- return 1;
- }
-
while(player.nextMsg()) {}
fclose(file);
Modified: pkg/trunk/util/logsetta/manifest.xml
===================================================================
--- pkg/trunk/util/logsetta/manifest.xml 2008-09-25 01:29:12 UTC (rev 4665)
+++ pkg/trunk/util/logsetta/manifest.xml 2008-09-25 02:16:41 UTC (rev 4666)
@@ -12,6 +12,7 @@
<license>BSD</license>
<depend package="roscpp"/>
<depend package="std_msgs"/>
+ <depend package="imu_node"/>
<depend package="logging"/>
<depend package="robot_msgs"/>
</package>
Modified: pkg/trunk/util/logsetta/odom/odom_extract.cpp
===================================================================
--- pkg/trunk/util/logsetta/odom/odom_extract.cpp 2008-09-25 01:29:12 UTC (rev 4665)
+++ pkg/trunk/util/logsetta/odom/odom_extract.cpp 2008-09-25 02:16:41 UTC (rev 4666)
@@ -36,10 +36,9 @@
#include <string>
#include "logging/LogPlayer.h"
-void odom_callback(std::string name, ros::msg* m, ros::Time t, void* f)
+void odom_callback(std::string name, std_msgs::RobotBase2DOdom* odom, ros::Time t, void* f)
{
FILE* file = (FILE*)f;
- std_msgs::RobotBase2DOdom* odom = (std_msgs::RobotBase2DOdom*)(m);
fprintf(file, "%.5f %.5f %.5f %.5f %.5f %.5f %.5f %.5f %d\n",
t.to_double(),
@@ -69,14 +68,8 @@
FILE* file = fopen("odom.txt", "w");
- count = player.addHandler<std_msgs::RobotBase2DOdom>(std::string("odom"), &odom_callback, file, true);
+ player.addHandler<std_msgs::RobotBase2DOdom>(std::string("*"), &odom_callback, file);
- if (count != 1)
- {
- printf("Found %d '/odom' topics when expecting 1", count);
- return 1;
- }
-
while(player.nextMsg()) {}
fclose(file);
Modified: pkg/trunk/util/megamaid/src/central_vac/central_vac.cpp
===================================================================
--- pkg/trunk/util/megamaid/src/central_vac/central_vac.cpp 2008-09-25 01:29:12 UTC (rev 4665)
+++ pkg/trunk/util/megamaid/src/central_vac/central_vac.cpp 2008-09-25 02:16:41 UTC (rev 4666)
@@ -36,8 +36,10 @@
#include <sys/stat.h>
#include "ros/node.h"
#include "logging/LogRecorder.h"
+#include "logging/AnyMsg.h"
#include <string>
+
using namespace std;
int main(int argc, char **argv)
@@ -73,7 +75,7 @@
for (vector<std::string>::iterator i = topics.begin(); i != topics.end(); i++)
{
printf("vacuum up [%s]\n", i->c_str());
- l.addTopic<AnyMsg>(*i);
+ l.addTopic<AnyMsg>(*i, 100);
}
l.start();
Modified: pkg/trunk/util/megamaid/src/dustbuster/dustbuster.cpp
===================================================================
--- pkg/trunk/util/megamaid/src/dustbuster/dustbuster.cpp 2008-09-25 01:29:12 UTC (rev 4665)
+++ pkg/trunk/util/megamaid/src/dustbuster/dustbuster.cpp 2008-09-25 02:16:41 UTC (rev 4666)
@@ -36,6 +36,7 @@
#include <sys/stat.h>
#include "ros/node.h"
#include "logging/LogRecorder.h"
+#include "logging/AnyMsg.h"
#include <string>
#include <rosthread/mutex.h>
@@ -44,11 +45,12 @@
ros::thread::mutex unsub_mutex;
unsigned int nCallbacks;
-void doUnsubscribe(string name, ros::msg* m, void* n)
+void doUnsubscribe(string name, ros::msg* m, ros::Time t, void* n)
{
+ unsub_mutex.lock();
- unsub_mutex.lock();
- //((ros::node*)(n))->unsubscribe(name);
+ ((ros::node*)(n))->unsubscribe(name);
+
cout << "Got a " << name << " message." << endl;
nCallbacks++;
@@ -57,6 +59,7 @@
((ros::node*)(n))->self_destruct();
}
unsub_mutex.unlock();
+
}
int main(int argc, char **argv)
@@ -143,7 +146,7 @@
for (vector<std::string>::iterator i = topics.begin(); i != topics.end(); i++)
{
printf("dustbustering up [%s]\n", i->c_str());
- l.addTopic<AnyMsg>(*i, &doUnsubscribe, (void*)(&n), false, 1);
+ l.addTopic<AnyMsg>(*i, 1, &doUnsubscribe, (void*)(&n), false);
}
l.start();
Modified: pkg/trunk/util/megamaid/src/playback/playback.cpp
===================================================================
--- pkg/trunk/util/megamaid/src/playback/playback.cpp 2008-09-25 01:29:12 UTC (rev 4665)
+++ pkg/trunk/util/megamaid/src/playback/playback.cpp 2008-09-25 02:16:41 UTC (rev 4666)
@@ -34,6 +34,7 @@
#include <string>
#include "logging/LogPlayer.h"
+#include "logging/AnyMsg.h"
using namespace std;
@@ -41,6 +42,9 @@
void doPublish(string name, ros::msg* m, ros::Time t, void* n)
{
+ if (((ros::node*)(n))->advertise<AnyMsg>(name, 0))
+ usleep(200000);
+
if(!g_at_once) {
ros::Time now = ros::Time::now();
ros::Duration delta = t - ros::Time::now();
@@ -69,34 +73,24 @@
MultiLogPlayer player;
- std::vector<std::string> topics;
+ std::vector<std::string> bags;
g_at_once = false;
for (int i = 1; i < argc; i++) {
if(!strcmp(argv[i], "--atonce"))
g_at_once = true;
else
- topics.push_back(argv[i]);
+ bags.push_back(argv[i]);
}
ros::Time start = ros::Time::now();
- if (player.open(topics, start))
+ if (player.open(bags, start))
{
- std::vector<std::string> names = player.getNames();
-
- for (std::vector<std::string>::iterator i = names.begin(); i != names.end(); i++)
- {
- n.advertise<AnyMsg>(*i, 1);
- }
-
player.addHandler<AnyMsg>(string("*"), &doPublish, (void*)(&n), false);
}
while(n.ok())
{
- if(g_at_once)
- usleep(100000);
-
if (!player.nextMsg())
{
n.self_destruct();
Modified: pkg/trunk/util/megamaid/src/stepback/stepback.cpp
===================================================================
--- pkg/trunk/util/megamaid/src/stepback/stepback.cpp 2008-09-25 01:29:12 UTC (rev 4665)
+++ pkg/trunk/util/megamaid/src/stepback/stepback.cpp 2008-09-25 02:16:41 UTC (rev 4666)
@@ -41,6 +41,11 @@
void doPublish(string name, ros::msg* m, ros::Time t, void* n)
{
+ if (((ros::node*)(n))->advertise<AnyMsg>(name, 0))
+ usleep(200000);
+
+ std::cout << "Hit enter to continue.";
+ std::cout.flush();
cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
((ros::node*)(n))->publish(name, *m);
@@ -60,21 +65,14 @@
MultiLogPlayer player;
- std::vector<std::string> topics;
+ std::vector<std::string> ags;
for (int i = 1; i < argc; i++)
- topics.push_back(argv[i]);
+ ags.push_back(argv[i]);
ros::Time start = ros::Time::now();
- if (player.open(topics, start))
+ if (player.open(ags, start))
{
- std::vector<std::string> names = player.getNames();
-
- for (std::vector<std::string>::iterator i = names.begin(); i != names.end(); i++)
- {
- n.advertise<AnyMsg>(*i, 1);
- }
-
player.addHandler<AnyMsg>(string("*"), &doPublish, (void*)(&n), false);
}
Modified: pkg/trunk/util/megamaid/src/vacuum/vacuum.cpp
===================================================================
--- pkg/trunk/util/megamaid/src/vacuum/vacuum.cpp 2008-09-25 01:29:12 UTC (rev 4665)
+++ pkg/trunk/util/megamaid/src/vacuum/vacuum.cpp 2008-09-25 02:16:41 UTC (rev 4666)
@@ -32,6 +32,7 @@
#include <sys/stat.h>
#include "ros/node.h"
#include "logging/LogRecorder.h"
+#include "logging/AnyMsg.h"
#include <string>
using namespace std;
@@ -81,7 +82,7 @@
std::string fname = std::string(logdir) + std::string("/") + sanitized + std::string(".bag");
if (l->open(fname, start))
{
- l->addTopic<AnyMsg>(*i);
+ l->addTopic<AnyMsg>(*i, 100);
logs.push_back(l);
} else {
cerr << "Could not open file: " << fname << endl;
@@ -89,11 +90,9 @@
}
}
-
for (vector<LogRecorder*>::iterator i = logs.begin(); i != logs.end(); i++)
(*i)->start();
-
if (logs.size() <= 0)
n.self_destruct();
Modified: pkg/trunk/vision/calib_converter/src/calib_converter.cpp
===================================================================
--- pkg/trunk/vision/calib_converter/src/calib_converter.cpp 2008-09-25 01:29:12 UTC (rev 4665)
+++ pkg/trunk/vision/calib_converter/src/calib_converter.cpp 2008-09-25 02:16:41 UTC (rev 4666)
@@ -11,8 +11,12 @@
#include <time.h>
#include "logging/LogPlayer.h"
+#include <set>
+
+
using namespace std;
+set<string> g_names;
void SplitFilename (const string& str, string *dir, string *file)
{
@@ -33,6 +37,7 @@
void copyMsg(string name, ros::msg* m, ros::Time t, void* n)
{
if (m != 0) {
+ g_names.insert(name);
*((T*)(n)) = *((T*)(m));
}
}
@@ -85,15 +90,16 @@
// -- If we don't have all the expected messages, yell.
bool hasImages = false, hasPtcld = false, hasCalparams = false;
- vector<string> names = lp.getNames();
- for (unsigned int i=0; i<names.size(); i++) {
- if(names[i].compare("videre/images") == 0) {
+
+ for (set<string>::iterator i = g_names.begin(); i != g_names.end(); i++)
+ {
+ if(*i == "videre/images") {
hasImages = true;
}
- else if(names[i].compare("videre/cal_params") == 0) {
+ else if (*i == "videre/cal_params") {
hasCalparams = true;
}
- else if(names[i].compare("full_cloud") == 0) {
+ else if(*i == "full_cloud") {
hasPtcld = true;
}
}
@@ -102,7 +108,6 @@
cerr << "Doing as much conversion as possible..." << endl;
}
-
string outputname;
// -- Save the pointcloud.
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
