fawkes-robotino-2016のgazebo9以降への対応

Ubuntu 18.04へのfawkes-robotino のインストール(試行錯誤版)にて,
[b-quote]gazebo 関係はgazebo5からgazebo9まで上がっていて,gazebo::physics:World のGetName などなどがなくなっているため,対応が面倒なのでいったんインストールリストから削除しました.[/b-quote]
とありましたが,その部分の答え合わせです(2016と2019のdiff)

diff -r fawkes-robotino/fawkes/src/plugins/gazebo/aspect/gazebo.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/aspect/gazebo.cpp
68c64,65
< GazeboAspect::init_GazeboAspect(gazebo::transport::NodePtr gazebonode , gazebo::transport::NodePtr gazebo_world_node)
---
> GazeboAspect::init_GazeboAspect(gazebo::transport::NodePtr gazebonode,
>                                 gazebo::transport::NodePtr gazebo_world_node)
70,71c67,68
<   this->gazebonode = gazebonode;
<   this->gazebo_world_node = gazebo_world_node;
---
> 	this->gazebonode        = gazebonode;
> 	this->gazebo_world_node = gazebo_world_node;
80c77
<   gazebonode.reset();
---
> 	gazebonode.reset();
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/aspect/gazebo.h fawkes-robotino-2016/fawkes/src/plugins/gazebo/aspect/gazebo.h
24,25c24,25
< #ifndef __PLUGINS_GAZEBO_ASPECT_GAZEBO_H_
< #define __PLUGINS_GAZEBO_ASPECT_GAZEBO_H_
---
> #ifndef _PLUGINS_GAZEBO_ASPECT_GAZEBO_H_
> #define _PLUGINS_GAZEBO_ASPECT_GAZEBO_H_
33,35c33,34
< #if 0 /* just to make Emacs auto-indent happy */
< }
< #endif
---
> 
> class GazeboAspectIniFin;
39c38
<   friend class GazeboAspectIniFin;
---
> 	friend GazeboAspectIniFin;
41,48c40,47
<  public:
<   GazeboAspect();
<   virtual ~GazeboAspect();
< 
<  protected:
<   /// Gazebo Node for communication with a robot
<   gazebo::transport::NodePtr gazebonode;
<   /**
---
> public:
> 	GazeboAspect();
> 	virtual ~GazeboAspect();
> 
> protected:
> 	/// Gazebo Node for communication with a robot
> 	gazebo::transport::NodePtr gazebonode;
> 	/**
52c51
<   gazebo::transport::NodePtr gazebo_world_node;
---
> 	gazebo::transport::NodePtr gazebo_world_node;
54,56c53,56
<  private:
<   void init_GazeboAspect(gazebo::transport::NodePtr gazebonode, gazebo::transport::NodePtr gazebo_world_node);
<   void finalize_GazeboAspect();
---
> private:
> 	void init_GazeboAspect(gazebo::transport::NodePtr gazebonode,
> 	                       gazebo::transport::NodePtr gazebo_world_node);
> 	void finalize_GazeboAspect();
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/aspect/gazebo_inifin.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/aspect/gazebo_inifin.cpp
24d23
< #include <plugins/gazebo/aspect/gazebo_inifin.h>
25a25,26
> #include <plugins/gazebo/aspect/gazebo_inifin.h>
> 
29,31d29
< #if 0 /* just to make Emacs auto-indent happy */
< }
< #endif
41,42c39
< GazeboAspectIniFin::GazeboAspectIniFin()
<   : AspectIniFin("GazeboAspect")
---
> GazeboAspectIniFin::GazeboAspectIniFin() : AspectIniFin("GazeboAspect")
52,61c49,59
<   GazeboAspect *gazebo_thread;
<   gazebo_thread = dynamic_cast<GazeboAspect *>(thread);
<   if (gazebo_thread == NULL) {
<     throw CannotInitializeThreadException("Thread '%s' claims to have the "
< 					  "GazeboAspect, but RTTI says it "
< 					  "has not. ", thread->name());
<   }
<   if (! __gazebonode) {
<     throw CannotInitializeThreadException("Gazebo node handle has not been set.");
<   }
---
> 	GazeboAspect *gazebo_thread;
> 	gazebo_thread = dynamic_cast<GazeboAspect *>(thread);
> 	if (gazebo_thread == NULL) {
> 		throw CannotInitializeThreadException("Thread '%s' claims to have the "
> 		                                      "GazeboAspect, but RTTI says it "
> 		                                      "has not. ",
> 		                                      thread->name());
> 	}
> 	if (!gazebonode_) {
> 		throw CannotInitializeThreadException("Gazebo node handle has not been set.");
> 	}
63c61
<   gazebo_thread->init_GazeboAspect(__gazebonode, __gazebo_world_node);
---
> 	gazebo_thread->init_GazeboAspect(gazebonode_, gazebo_world_node_);
72,79c70,78
<   GazeboAspect *gazebo_thread;
<   gazebo_thread = dynamic_cast<GazeboAspect *>(thread);
<   if (gazebo_thread == NULL) {
<     throw CannotFinalizeThreadException("Thread '%s' claims to have the "
< 					"GazeboAspect, but RTTI says it "
< 					"has not. ", thread->name());
<   }
<   gazebo_thread->finalize_GazeboAspect();
---
> 	GazeboAspect *gazebo_thread;
> 	gazebo_thread = dynamic_cast<GazeboAspect *>(thread);
> 	if (gazebo_thread == NULL) {
> 		throw CannotFinalizeThreadException("Thread '%s' claims to have the "
> 		                                    "GazeboAspect, but RTTI says it "
> 		                                    "has not. ",
> 		                                    thread->name());
> 	}
> 	gazebo_thread->finalize_GazeboAspect();
82d80
< 
90c88
<   __gazebonode = gazebonode;
---
> 	gazebonode_ = gazebonode;
99c97
<   __gazebo_world_node = gazebo_world_node;
---
> 	gazebo_world_node_ = gazebo_world_node;
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/aspect/gazebo_inifin.h fawkes-robotino-2016/fawkes/src/plugins/gazebo/aspect/gazebo_inifin.h
24,25c24,25
< #ifndef __PLUGINS_GAZEBO_ASPECT_GAZEBO_INIFIN_H_
< #define __PLUGINS_GAZEBO_ASPECT_GAZEBO_INIFIN_H_
---
> #ifndef _PLUGINS_GAZEBO_ASPECT_GAZEBO_INIFIN_H_
> #define _PLUGINS_GAZEBO_ASPECT_GAZEBO_INIFIN_H_
34,36d33
< #if 0 /* just to make Emacs auto-indent happy */
< }
< #endif
40,41c37,38
<  public:
<   GazeboAspectIniFin();
---
> public:
> 	GazeboAspectIniFin();
43,52c40,41
<   virtual void init(Thread *thread);
<   virtual void finalize(Thread *thread);
< 
<   //setters for the node_thread
<   void set_gazebonode(gazebo::transport::NodePtr gazebonode);
<   void set_gazebo_world_node(gazebo::transport::NodePtr gazebo_world_node);
< 
<  private:
<   gazebo::transport::NodePtr __gazebonode;
<   gazebo::transport::NodePtr __gazebo_world_node;
---
> 	virtual void init(Thread *thread);
> 	virtual void finalize(Thread *thread);
53a43,49
> 	//setters for the node_thread
> 	void set_gazebonode(gazebo::transport::NodePtr gazebonode);
> 	void set_gazebo_world_node(gazebo::transport::NodePtr gazebo_world_node);
> 
> private:
> 	gazebo::transport::NodePtr gazebonode_;
> 	gazebo::transport::NodePtr gazebo_world_node_;
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/aspect/Makefile fawkes-robotino-2016/fawkes/src/plugins/gazebo/aspect/Makefile
30c30,31
< OBJS_all    = $(OBJS_libfawkesgazeboaspect)
---
> OBJS_all = $(OBJS_libfawkesgazeboaspect)
> LIBS_all = $(LIBDIR)/libfawkesgazeboaspect.so
33c34
<   LIBS_all = $(LIBDIR)/libfawkesgazeboaspect.so
---
>   LIBS_build = $(LIBS_all)
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazebo.mk fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazebo.mk
15a16,17
> include $(BUILDSYSDIR)/boost.mk
> 
25a28
>   HAVE_GAZEBO_96 = $(if $(shell $(PKGCONFIG) --atleast-version=9.6.0 'gazebo'; echo $${?/1/}),1,0)
29c32,35
<   CFLAGS_GAZEBO  = -DHAVE_GAZEBO $(shell $(PKGCONFIG) --cflags 'gazebo')
---
>   # Gazebo 8 declared several symbols as deprecated but still uses them.
>   # Disable the deprecated declarations warning until this is fixed.
>   CFLAGS_GAZEBO  = -DHAVE_GAZEBO $(shell $(PKGCONFIG) --cflags 'gazebo') \
>                    -Wno-deprecated-declarations
30a37,40
> 
>   ifeq ($(HAVE_GAZEBO_96)$(boost-have-lib system),11)
>     LDFLAGS_GAZEBO += $(boost-lib-ldflags system)
>   endif
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazebo_plugin.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazebo_plugin.cpp
26d25
< #include <gazebo/transport/TransportIface.hh>
28a28,29
> #include <gazebo/transport/TransportIface.hh>
> 
41,42c42
< GazeboPlugin::GazeboPlugin(Configuration *config)
<   : Plugin(config)
---
> GazeboPlugin::GazeboPlugin(Configuration *config) : Plugin(config)
44c44
<   thread_list.push_back(new GazeboNodeThread());
---
> 	thread_list.push_back(new GazeboNodeThread());
46d45
< 
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazebo_plugin.h fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazebo_plugin.h
23,24c23,24
< #ifndef __PLUGINS_GAZEBO_GAZEBO_PLUGIN_H_
< #define __PLUGINS_GAZEBO_GAZEBO_PLUGIN_H_
---
> #ifndef _PLUGINS_GAZEBO_GAZEBO_PLUGIN_H_
> #define _PLUGINS_GAZEBO_GAZEBO_PLUGIN_H_
30,31c30,31
<  public:
<   GazeboPlugin(fawkes::Configuration *config);
---
> public:
> 	explicit GazeboPlugin(fawkes::Configuration *config);
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-comm/gazsim_comm_plugin.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-comm/gazsim_comm_plugin.cpp
25a26
> 
35,36c36,37
<  public:
<   /** Constructor.
---
> public:
> 	/** Constructor.
39,42c40,43
<   GazsimCommPlugin(Configuration *config) : Plugin(config)
<   {
<     thread_list.push_back(new GazsimCommThread());
<   }
---
> 	explicit GazsimCommPlugin(Configuration *config) : Plugin(config)
> 	{
> 		thread_list.push_back(new GazsimCommThread());
> 	}
44d44
< 
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-comm/gazsim_comm_thread.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-comm/gazsim_comm_thread.cpp
24a25,26
> #include "gazsim_comm_thread.h"
> 
26d27
< #include <protobuf_comm/peer.h>
28,29c29,30
< #include <stdlib.h>
< #include "gazsim_comm_thread.h"
---
> #include <protobuf_comm/peer.h>
> 
30a32
> #include <stdlib.h>
41,42c43,44
<   : Thread("GazsimCommThread", Thread::OPMODE_WAITFORWAKEUP),
<     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE)
---
> : Thread("GazsimCommThread", Thread::OPMODE_WAITFORWAKEUP),
>   BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE)
50d51
< 
54,55c55,56
<   //logger->log_info(name(), "GazsimComm initializing");
<   initialized_ = false;
---
> 	//logger->log_info(name(), "GazsimComm initializing");
> 	initialized_ = false;
57,128c58,136
<   //read config values
<   proto_dirs_ = config->get_strings("/gazsim/proto-dirs");
<   package_loss_ = config->get_float("/gazsim/comm/package-loss");
<   addresses_ = config->get_strings("/gazsim/comm/addresses");
<   send_ports_ = config->get_uints("/gazsim/comm/send-ports");
<   recv_ports_ = config->get_uints("/gazsim/comm/recv-ports");
<   use_crypto1_ = config->get_bool("/gazsim/comm/use-crypto1");
<   use_crypto2_ = config->get_bool("/gazsim/comm/use-crypto1");
<   send_ports_crypto1_ = config->get_uints("/gazsim/comm/send-ports-crypto1");
<   recv_ports_crypto1_ = config->get_uints("/gazsim/comm/recv-ports-crypto1");
<   send_ports_crypto2_ = config->get_uints("/gazsim/comm/send-ports-crypto2");
<   recv_ports_crypto2_ = config->get_uints("/gazsim/comm/recv-ports-crypto2");
<   if(addresses_.size() != send_ports_.size() || addresses_.size() != recv_ports_.size()
<      || ( use_crypto1_ && addresses_.size() != send_ports_crypto1_.size())
<      || ( use_crypto1_ && addresses_.size() != recv_ports_crypto1_.size())
<      || ( use_crypto2_ && addresses_.size() != send_ports_crypto2_.size())
<      || ( use_crypto2_ && addresses_.size() != recv_ports_crypto2_.size()))
<   {
<     logger->log_warn(name(), "/gazsim/comm/ has an invalid configuration!");
<   }
< 
< 
< 
<   //resolve proto paths
<   try {
<     proto_dirs_ = config->get_strings("/clips-protobuf/proto-dirs");
<     for (size_t i = 0; i < proto_dirs_.size(); ++i) {
<       std::string::size_type pos;
<       if ((pos = proto_dirs_[i].find("@BASEDIR@")) != std::string::npos) {
< 	proto_dirs_[i].replace(pos, 9, BASEDIR);
<       }
<       if ((pos = proto_dirs_[i].find("@FAWKES_BASEDIR@")) != std::string::npos) {
< 	proto_dirs_[i].replace(pos, 16, FAWKES_BASEDIR);
<       }
<       if ((pos = proto_dirs_[i].find("@RESDIR@")) != std::string::npos) {
< 	proto_dirs_[i].replace(pos, 8, RESDIR);
<       }
<       if ((pos = proto_dirs_[i].find("@CONFDIR@")) != std::string::npos) {
< 	proto_dirs_[i].replace(pos, 9, CONFDIR);
<       }
<       if (proto_dirs_[i][proto_dirs_.size()-1] != '/') {
< 	proto_dirs_[i] += "/";
<       }
<     }
<   } catch (Exception &e) {
<     logger->log_warn(name(), "Failed to load proto paths from config, exception follows");
<     logger->log_warn(name(), e);
<   }
< 
<   //create peer connections 
<   peers_.resize(addresses_.size());
<   peers_crypto1_.resize(addresses_.size());
<   peers_crypto2_.resize(addresses_.size());
<   for(unsigned int i = 0; i < addresses_.size(); i++)
<   {
<     peers_[i] = new ProtobufBroadcastPeer(addresses_[i], send_ports_[i],
< 					  recv_ports_[i], proto_dirs_);
<     peers_[i]->signal_received_raw().connect(boost::bind(&GazsimCommThread::receive_raw_msg, this, _1, _2, _3, _4));
<     if(use_crypto1_)
<     {
<       peers_crypto1_[i] = new ProtobufBroadcastPeer(addresses_[i], send_ports_crypto1_[i],
< 					  recv_ports_crypto1_[i], proto_dirs_);
<       peers_crypto1_[i]->signal_received_raw().connect(boost::bind(&GazsimCommThread::receive_raw_msg, this, _1, _2, _3, _4));
<     }
<     if(use_crypto2_)
<     {
<       peers_crypto2_[i] = new ProtobufBroadcastPeer(addresses_[i], send_ports_crypto2_[i],
< 						    recv_ports_crypto2_[i], proto_dirs_);
<       peers_crypto2_[i]->signal_received_raw().connect(boost::bind(&GazsimCommThread::receive_raw_msg, this, _1, _2, _3, _4));
<     }
<   }
<   initialized_ = true;
---
> 	//read config values
> 	proto_dirs_         = config->get_strings("/gazsim/proto-dirs");
> 	package_loss_       = config->get_float("/gazsim/comm/package-loss");
> 	addresses_          = config->get_strings("/gazsim/comm/addresses");
> 	send_ports_         = config->get_uints("/gazsim/comm/send-ports");
> 	recv_ports_         = config->get_uints("/gazsim/comm/recv-ports");
> 	use_crypto1_        = config->get_bool("/gazsim/comm/use-crypto1");
> 	use_crypto2_        = config->get_bool("/gazsim/comm/use-crypto1");
> 	send_ports_crypto1_ = config->get_uints("/gazsim/comm/send-ports-crypto1");
> 	recv_ports_crypto1_ = config->get_uints("/gazsim/comm/recv-ports-crypto1");
> 	send_ports_crypto2_ = config->get_uints("/gazsim/comm/send-ports-crypto2");
> 	recv_ports_crypto2_ = config->get_uints("/gazsim/comm/recv-ports-crypto2");
> 	if (addresses_.size() != send_ports_.size() || addresses_.size() != recv_ports_.size()
> 	    || (use_crypto1_ && addresses_.size() != send_ports_crypto1_.size())
> 	    || (use_crypto1_ && addresses_.size() != recv_ports_crypto1_.size())
> 	    || (use_crypto2_ && addresses_.size() != send_ports_crypto2_.size())
> 	    || (use_crypto2_ && addresses_.size() != recv_ports_crypto2_.size())) {
> 		logger->log_warn(name(), "/gazsim/comm/ has an invalid configuration!");
> 	}
> 
> 	//resolve proto paths
> 	try {
> 		proto_dirs_ = config->get_strings("/clips-protobuf/proto-dirs");
> 		for (size_t i = 0; i < proto_dirs_.size(); ++i) {
> 			std::string::size_type pos;
> 			if ((pos = proto_dirs_[i].find("@BASEDIR@")) != std::string::npos) {
> 				proto_dirs_[i].replace(pos, 9, BASEDIR);
> 			}
> 			if ((pos = proto_dirs_[i].find("@FAWKES_BASEDIR@")) != std::string::npos) {
> 				proto_dirs_[i].replace(pos, 16, FAWKES_BASEDIR);
> 			}
> 			if ((pos = proto_dirs_[i].find("@RESDIR@")) != std::string::npos) {
> 				proto_dirs_[i].replace(pos, 8, RESDIR);
> 			}
> 			if ((pos = proto_dirs_[i].find("@CONFDIR@")) != std::string::npos) {
> 				proto_dirs_[i].replace(pos, 9, CONFDIR);
> 			}
> 			if (proto_dirs_[i][proto_dirs_.size() - 1] != '/') {
> 				proto_dirs_[i] += "/";
> 			}
> 		}
> 	} catch (Exception &e) {
> 		logger->log_warn(name(), "Failed to load proto paths from config, exception follows");
> 		logger->log_warn(name(), e);
> 	}
> 
> 	//create peer connections
> 	peers_.resize(addresses_.size());
> 	peers_crypto1_.resize(addresses_.size());
> 	peers_crypto2_.resize(addresses_.size());
> 	for (unsigned int i = 0; i < addresses_.size(); i++) {
> 		peers_[i] =
> 		  new ProtobufBroadcastPeer(addresses_[i], send_ports_[i], recv_ports_[i], proto_dirs_);
> 		peers_[i]->signal_received_raw().connect(
> 		  boost::bind(&GazsimCommThread::receive_raw_msg, this, _1, _2, _3, _4));
> 		peers_[i]->signal_send_error().connect(
> 		  boost::bind(&GazsimCommThread::peer_send_error, this, addresses_[i], send_ports_[i], _1));
> 		if (use_crypto1_) {
> 			peers_crypto1_[i] = new ProtobufBroadcastPeer(addresses_[i],
> 			                                              send_ports_crypto1_[i],
> 			                                              recv_ports_crypto1_[i],
> 			                                              proto_dirs_);
> 			peers_crypto1_[i]->signal_received_raw().connect(
> 			  boost::bind(&GazsimCommThread::receive_raw_msg, this, _1, _2, _3, _4));
> 			peers_crypto1_[i]->signal_send_error().connect(boost::bind(
> 			  &GazsimCommThread::peer_send_error, this, addresses_[i], send_ports_crypto1_[i], _1));
> 		}
> 		if (use_crypto2_) {
> 			peers_crypto2_[i] = new ProtobufBroadcastPeer(addresses_[i],
> 			                                              send_ports_crypto2_[i],
> 			                                              recv_ports_crypto2_[i],
> 			                                              proto_dirs_);
> 			peers_crypto2_[i]->signal_received_raw().connect(
> 			  boost::bind(&GazsimCommThread::receive_raw_msg, this, _1, _2, _3, _4));
> 			peers_crypto2_[i]->signal_send_error().connect(boost::bind(
> 			  &GazsimCommThread::peer_send_error, this, addresses_[i], send_ports_crypto2_[i], _1));
> 		}
> 	}
> 	initialized_ = true;
131d138
< 
135,138c142,144
<   for(unsigned int i = 0; i < peers_.size(); i++)
<   {
<     delete peers_[i];
<   }
---
> 	for (unsigned int i = 0; i < peers_.size(); i++) {
> 		delete peers_[i];
> 	}
141d146
< 
148,183d152
<  * Receive and forward msg
<  * @param endpoint port msg received from
<  * @param component_id message_component_id
<  * @param msg_type msg_type
<  * @param msg Message
<  */
< void
< GazsimCommThread::receive_msg(boost::asio::ip::udp::endpoint &endpoint,
< 		       uint16_t component_id, uint16_t msg_type,
< 		       std::shared_ptr<google::protobuf::Message> msg)
< {
<   //logger->log_info(name(), "Got Peer Message from port %d", endpoint.port());
<   unsigned int incoming_peer_port = endpoint.port(); //this is suprisingly the send-port
<  
<   if(!initialized_)
<   {
<     return;
<   }
< 
<   //simulate package loss
<   double rnd = ((double) rand()) / ((double) RAND_MAX); //0.0 <= rnd <= 1.0
<   if(rnd < package_loss_)
<   {
<     return;
<   }
<   //send message to all other peers
<   for(unsigned int i = 0; i < peers_.size(); i++)
<   {
<     if(send_ports_[i] != incoming_peer_port)
<     {
<       peers_[i]->send(msg);
<     }
<   }
< }
< 
< /**
192,193c161,209
< 				  protobuf_comm::frame_header_t &header, void * data,
< 				  size_t length)
---
>                                   protobuf_comm::frame_header_t & header,
>                                   void *                          data,
>                                   size_t                          length)
> {
> 	//logger->log_info(name(), "Got raw Message from port %d", endpoint.port());
> 	unsigned int incoming_peer_port = endpoint.port(); //this is suprisingly the send-port
> 
> 	if (!initialized_) {
> 		return;
> 	}
> 
> 	//simulate package loss
> 	double rnd = ((double)rand()) / ((double)RAND_MAX); //0.0 <= rnd <= 1.0
> 	if (rnd < package_loss_) {
> 		return;
> 	}
> 
> 	//check which set of peers the message comes from
> 	std::vector<protobuf_comm::ProtobufBroadcastPeer *> peers;
> 	std::vector<unsigned int>                           send_ports;
> 	if (std::find(send_ports_.begin(), send_ports_.end(), incoming_peer_port) != send_ports_.end()) {
> 		peers      = peers_;
> 		send_ports = send_ports_;
> 	} else if (use_crypto1_
> 	           && std::find(send_ports_crypto1_.begin(),
> 	                        send_ports_crypto1_.end(),
> 	                        incoming_peer_port)
> 	                != send_ports_crypto1_.end()) {
> 		peers      = peers_crypto1_;
> 		send_ports = send_ports_crypto1_;
> 	} else if (use_crypto2_
> 	           && std::find(send_ports_crypto2_.begin(),
> 	                        send_ports_crypto2_.end(),
> 	                        incoming_peer_port)
> 	                != send_ports_crypto2_.end()) {
> 		peers      = peers_crypto2_;
> 		send_ports = send_ports_crypto2_;
> 	}
> 
> 	//send message to all other peers
> 	for (unsigned int i = 0; i < peers.size(); i++) {
> 		if (send_ports[i] != incoming_peer_port) {
> 			peers[i]->send_raw(header, data, length);
> 		}
> 	}
> }
> 
> void
> GazsimCommThread::peer_send_error(std::string address, unsigned int port, std::string err)
195,236c211
<   //logger->log_info(name(), "Got raw Message from port %d", endpoint.port());
<   unsigned int incoming_peer_port = endpoint.port(); //this is suprisingly the send-port
<  
<   if(!initialized_)
<   {
<     return;
<   }
< 
<   //simulate package loss
<   double rnd = ((double) rand()) / ((double) RAND_MAX); //0.0 <= rnd <= 1.0
<   if(rnd < package_loss_)
<   {
<     return;
<   }
< 
<   //check which set of peers the message comes from
<   std::vector<protobuf_comm::ProtobufBroadcastPeer*> peers;
<   std::vector<unsigned int> send_ports;
<   if(std::find(send_ports_.begin(), send_ports_.end(), incoming_peer_port) != send_ports_.end())
<   {
<     peers = peers_;
<     send_ports = send_ports_;
<   }
<   else if(use_crypto1_ && std::find(send_ports_crypto1_.begin(), send_ports_crypto1_.end(), incoming_peer_port) != send_ports_crypto1_.end())
<   {
<     peers = peers_crypto1_;
<     send_ports = send_ports_crypto1_;
<   }  
<   else if(use_crypto2_ && std::find(send_ports_crypto2_.begin(), send_ports_crypto2_.end(), incoming_peer_port) != send_ports_crypto1_.end())
<   {
<     peers = peers_crypto2_;
<     send_ports = send_ports_crypto2_;
<   }
< 
<   //send message to all other peers
<   for(unsigned int i = 0; i < peers.size(); i++)
<   {
<     if(send_ports[i] != incoming_peer_port)
<     {
<       peers[i]->send_raw(header, data, length);
<     }
<   }
---
> 	logger->log_warn(name(), "Peer send error for %s:%u: %s", address.c_str(), port, err.c_str());
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-comm/gazsim_comm_thread.h fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-comm/gazsim_comm_thread.h
25,26c25,26
< #ifndef __PLUGINS_GAZSIM_COMM_COMM_THREAD_H_
< #define __PLUGINS_GAZSIM_COMM_COMM_THREAD_H_
---
> #ifndef _PLUGINS_GAZSIM_COMM_COMM_THREAD_H_
> #define _PLUGINS_GAZSIM_COMM_COMM_THREAD_H_
28,30d27
< #include <core/threading/thread.h>
< #include <aspect/logging.h>
< #include <aspect/configurable.h>
32c29,31
< #include <boost/asio.hpp>
---
> #include <aspect/configurable.h>
> #include <aspect/logging.h>
> #include <core/threading/thread.h>
34d32
< #include <protobuf_comm/peer.h>
36c34
< #include <list>
---
> #include <protobuf_comm/peer.h>
37a36,37
> #include <boost/asio.hpp>
> #include <list>
40c40
<   class ProtobufStreamClient;
---
> class ProtobufStreamClient;
43,47c43,46
< class GazsimCommThread
< : public fawkes::Thread,
<   public fawkes::BlockedTimingAspect,
<   public fawkes::ConfigurableAspect,
<   public fawkes::LoggingAspect
---
> class GazsimCommThread : public fawkes::Thread,
>                          public fawkes::BlockedTimingAspect,
>                          public fawkes::ConfigurableAspect,
>                          public fawkes::LoggingAspect
49,81c48,85
<  public:
<   GazsimCommThread();
<   ~GazsimCommThread();
< 
<   virtual void init();
<   virtual void loop();
<   virtual void finalize();
< 
<   void receive_msg(boost::asio::ip::udp::endpoint &endpoint,
< 		       uint16_t component_id, uint16_t msg_type,
< 		       std::shared_ptr<google::protobuf::Message> msg);
<   void receive_raw_msg(boost::asio::ip::udp::endpoint &endpoint,
< 		       protobuf_comm::frame_header_t &header, void * data,
< 		       size_t length);
< 
<  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
<  protected: virtual void run() { Thread::run(); }
< 
<  private:
<   std::vector<protobuf_comm::ProtobufBroadcastPeer*> peers_;
<   std::vector<protobuf_comm::ProtobufBroadcastPeer*> peers_crypto1_;
<   std::vector<protobuf_comm::ProtobufBroadcastPeer*> peers_crypto2_;
< 
<   //config values
<   std::vector<std::string> addresses_;
<   std::vector<unsigned int> send_ports_;
<   std::vector<unsigned int> recv_ports_;
<   std::vector<unsigned int> send_ports_crypto1_;
<   std::vector<unsigned int> recv_ports_crypto1_;
<   std::vector<unsigned int> send_ports_crypto2_;
<   std::vector<unsigned int> recv_ports_crypto2_;
<   
<   bool use_crypto1_, use_crypto2_;
---
> public:
> 	GazsimCommThread();
> 	~GazsimCommThread();
> 
> 	virtual void init();
> 	virtual void loop();
> 	virtual void finalize();
> 
> 	/** Stub to see name in backtrace for easier debugging. @see Thread::run() */
> protected:
> 	virtual void
> 	run()
> 	{
> 		Thread::run();
> 	}
> 
> private:
> 	void peer_send_error(std::string address, unsigned int port, std::string err);
> 	void receive_raw_msg(boost::asio::ip::udp::endpoint &endpoint,
> 	                     protobuf_comm::frame_header_t & header,
> 	                     void *                          data,
> 	                     size_t                          length);
> 
> private:
> 	std::vector<protobuf_comm::ProtobufBroadcastPeer *> peers_;
> 	std::vector<protobuf_comm::ProtobufBroadcastPeer *> peers_crypto1_;
> 	std::vector<protobuf_comm::ProtobufBroadcastPeer *> peers_crypto2_;
> 
> 	//config values
> 	std::vector<std::string>  addresses_;
> 	std::vector<unsigned int> send_ports_;
> 	std::vector<unsigned int> recv_ports_;
> 	std::vector<unsigned int> send_ports_crypto1_;
> 	std::vector<unsigned int> recv_ports_crypto1_;
> 	std::vector<unsigned int> send_ports_crypto2_;
> 	std::vector<unsigned int> recv_ports_crypto2_;
> 
> 	bool use_crypto1_, use_crypto2_;
83,84c87,88
<   std::vector<std::string> proto_dirs_;
<   double package_loss_;
---
> 	std::vector<std::string> proto_dirs_;
> 	double                   package_loss_;
86,87c90,91
<   //helper variables
<   bool initialized_;
---
> 	//helper variables
> 	bool initialized_;
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-comm/Makefile fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-comm/Makefile
28c28,29
< LIBS_gazsim_comm = fawkescore fawkesutils fawkesaspects fawkes_protobuf_comm fawkesgazeboaspect gazsim_msgs 
---
> LIBS_gazsim_comm = fawkescore fawkesutils fawkesaspects fawkes_protobuf_comm \
>                    fawkesgazeboaspect gazsim_msgs 
31c32,33
< OBJS_all = $(OBJS_gazsim_comm)
---
> OBJS_all    = $(OBJS_gazsim_comm)
> PLUGINS_all = $(PLUGINDIR)/gazsim-comm.so
42c44
<   PLUGINS_all = $(PLUGINDIR)/gazsim-comm.so
---
>   PLUGINS_build = $(PLUGINS_all)
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-depthcam/gazsim_depthcam_plugin.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-depthcam/gazsim_depthcam_plugin.cpp
22,23d21
< #include <core/plugin.h>
< 
25a24,25
> #include <core/plugin.h>
> 
35,37c35,36
<  public:
< 
<   /** Constructor.
---
> public:
> 	/** Constructor.
40,44c39,42
<   GazsimDepthcamPlugin(Configuration *config)
<     : Plugin(config)
<   {
<     thread_list.push_back(new DepthcamSimThread());
<   }
---
> 	explicit GazsimDepthcamPlugin(Configuration *config) : Plugin(config)
> 	{
> 		thread_list.push_back(new DepthcamSimThread());
> 	}
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-depthcam/gazsim_depthcam_thread.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-depthcam/gazsim_depthcam_thread.cpp
23a24
> #include <aspect/logging.h>
25,26d25
< #include <stdio.h>
< #include <math.h>
29d27
< #include <gazebo/transport/Node.hh>
30a29
> #include <gazebo/transport/Node.hh>
32c31,32
< #include <aspect/logging.h>
---
> #include <math.h>
> #include <stdio.h>
44,45c44,45
<   : Thread("DepthcamSimThread", Thread::OPMODE_WAITFORWAKEUP),
<     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)
---
> : Thread("DepthcamSimThread", Thread::OPMODE_WAITFORWAKEUP),
>   BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)
49c49,50
< void DepthcamSimThread::init()
---
> void
> DepthcamSimThread::init()
51,66c52,68
<   logger->log_debug(name(), "Initializing Simulation of the Depthcam");
<   topic_name_ = config->get_string("/gazsim/depthcam/topic");
<   width_ = config->get_float("/gazsim/depthcam/width");
<   height_ = config->get_float("/gazsim/depthcam/height");
<   frame_ = config->get_string("/gazsim/depthcam/frame");
<   pcl_id_ = config->get_string("/gazsim/depthcam/pointcloud-id");
< 
<   depthcam_sub_ = gazebo_world_node->Subscribe(topic_name_, &DepthcamSimThread::on_depthcam_data_msg, this);
< 
<   //create pointcloud:
<   pcl_ = new pcl::PointCloud<pcl::PointXYZ>();
<   pcl_->is_dense = false;
<   pcl_->width    = width_;
<   pcl_->height   = height_;
<   pcl_->points.resize(width_ * height_);
<   pcl_->header.frame_id = frame_;
---
> 	logger->log_debug(name(), "Initializing Simulation of the Depthcam");
> 	topic_name_ = config->get_string("/gazsim/depthcam/topic");
> 	width_      = config->get_float("/gazsim/depthcam/width");
> 	height_     = config->get_float("/gazsim/depthcam/height");
> 	frame_      = config->get_string("/gazsim/depthcam/frame");
> 	pcl_id_     = config->get_string("/gazsim/depthcam/pointcloud-id");
> 
> 	depthcam_sub_ =
> 	  gazebo_world_node->Subscribe(topic_name_, &DepthcamSimThread::on_depthcam_data_msg, this);
> 
> 	//create pointcloud:
> 	pcl_           = new pcl::PointCloud<pcl::PointXYZ>();
> 	pcl_->is_dense = false;
> 	pcl_->width    = width_;
> 	pcl_->height   = height_;
> 	pcl_->points.resize((size_t)width_ * (size_t)height_);
> 	pcl_->header.frame_id = frame_;
68c70
<   pcl_manager->add_pointcloud(pcl_id_.c_str(), pcl_);
---
> 	pcl_manager->add_pointcloud(pcl_id_.c_str(), pcl_);
71c73,74
< void DepthcamSimThread::finalize()
---
> void
> DepthcamSimThread::finalize()
73c76
<   pcl_manager->remove_pointcloud(pcl_id_.c_str());
---
> 	pcl_manager->remove_pointcloud(pcl_id_.c_str());
76c79,80
< void DepthcamSimThread::loop()
---
> void
> DepthcamSimThread::loop()
78c82
<   //The interesting stuff happens in the callback of the depthcam
---
> 	//The interesting stuff happens in the callback of the depthcam
81,104c85,109
< void DepthcamSimThread::on_depthcam_data_msg(ConstPointCloudPtr &msg)
< {  
<   // logger->log_info(name(), "Got Point Cloud!");
< 
<   //only write when pcl is used
<   // if (pcl_.use_count() > 1)
<   // {
<     fawkes::Time capture_time = clock->now();
< 
<     pcl::PointCloud<pcl::PointXYZ> &pcl = **pcl_;
<     pcl.header.seq += 1;
<     pcl_utils::set_time(pcl_, capture_time);
< 
<     //insert or update points in pointcloud
<     unsigned int idx = 0;
<     for (unsigned int h = 0; h < height_; ++h) {
<       for (unsigned int w = 0; w < width_; ++w, ++idx) {
<         // Fill in XYZ
<         pcl.points[idx].x = msg->points(idx).z();
<         pcl.points[idx].y = -msg->points(idx).x();
<         pcl.points[idx].z = msg->points(idx).y();
<       }
<     }
<   // }
---
> void
> DepthcamSimThread::on_depthcam_data_msg(ConstPointCloudPtr &msg)
> {
> 	// logger->log_info(name(), "Got Point Cloud!");
> 
> 	//only write when pcl is used
> 	// if (pcl_.use_count() > 1)
> 	// {
> 	fawkes::Time capture_time = clock->now();
> 
> 	pcl::PointCloud<pcl::PointXYZ> &pcl = **pcl_;
> 	pcl.header.seq += 1;
> 	pcl_utils::set_time(pcl_, capture_time);
> 
> 	//insert or update points in pointcloud
> 	unsigned int idx = 0;
> 	for (unsigned int h = 0; h < height_; ++h) {
> 		for (unsigned int w = 0; w < width_; ++w, ++idx) {
> 			// Fill in XYZ
> 			pcl.points[idx].x = msg->points(idx).z();
> 			pcl.points[idx].y = -msg->points(idx).x();
> 			pcl.points[idx].z = msg->points(idx).y();
> 		}
> 	}
> 	// }
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-depthcam/gazsim_depthcam_thread.h fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-depthcam/gazsim_depthcam_thread.h
22,23c22,23
< #ifndef __PLUGINS_GAZSIM_DEPTHCAM_THREAD_H_
< #define __PLUGINS_GAZSIM_DEPTHCAM_THREAD_H_
---
> #ifndef _PLUGINS_GAZSIM_DEPTHCAM_THREAD_H_
> #define _PLUGINS_GAZSIM_DEPTHCAM_THREAD_H_
25c25,26
< #include <core/threading/thread.h>
---
> #include <aspect/blackboard.h>
> #include <aspect/blocked_timing.h>
29,33d29
< #include <aspect/blackboard.h>
< #include <aspect/blocked_timing.h>
< #include <plugins/gazebo/aspect/gazebo.h>
< #include <string.h>
< 
34a31
> #include <core/threading/thread.h>
36a34
> #include <plugins/gazebo/aspect/gazebo.h>
37a36
> #include <string.h>
40d38
< #include <gazebo/transport/TransportTypes.hh>
41a40
> #include <gazebo/transport/TransportTypes.hh>
44,52c43,50
< class DepthcamSimThread
< : public fawkes::Thread,
<   public fawkes::ClockAspect,
<   public fawkes::LoggingAspect,
<   public fawkes::ConfigurableAspect,
<   public fawkes::BlackBoardAspect,
<   public fawkes::BlockedTimingAspect,
<   public fawkes::GazeboAspect,
<   public fawkes::PointCloudAspect
---
> class DepthcamSimThread : public fawkes::Thread,
>                           public fawkes::ClockAspect,
>                           public fawkes::LoggingAspect,
>                           public fawkes::ConfigurableAspect,
>                           public fawkes::BlackBoardAspect,
>                           public fawkes::BlockedTimingAspect,
>                           public fawkes::GazeboAspect,
>                           public fawkes::PointCloudAspect
54,55c52,53
<  public:
<   DepthcamSimThread();
---
> public:
> 	DepthcamSimThread();
57,75c55,73
<   virtual void init();
<   virtual void loop();
<   virtual void finalize();
< 
<  private:
<   gazebo::transport::SubscriberPtr depthcam_sub_;
< 
<   //handler function for incoming depthcam data messages
<   void on_depthcam_data_msg(ConstPointCloudPtr &msg);
< 
<   //config values
<   //topic name of the gazebo message publisher
<   std::string topic_name_;
<   unsigned int width_;
<   unsigned int height_;
<   //id of the shared memory object
<   std::string shm_id_;
<   std::string frame_;
<   std::string pcl_id_;
---
> 	virtual void init();
> 	virtual void loop();
> 	virtual void finalize();
> 
> private:
> 	gazebo::transport::SubscriberPtr depthcam_sub_;
> 
> 	//handler function for incoming depthcam data messages
> 	void on_depthcam_data_msg(ConstPointCloudPtr &msg);
> 
> 	//config values
> 	//topic name of the gazebo message publisher
> 	std::string  topic_name_;
> 	unsigned int width_;
> 	unsigned int height_;
> 	//id of the shared memory object
> 	std::string shm_id_;
> 	std::string frame_;
> 	std::string pcl_id_;
77c75
<   fawkes::RefPtr<pcl::PointCloud<pcl::PointXYZ> > pcl_;
---
> 	fawkes::RefPtr<pcl::PointCloud<pcl::PointXYZ>> pcl_;
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-depthcam/Makefile fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-depthcam/Makefile
26,28c26,28
< LIBS_gazsim_depthcam = fawkescore fawkesutils fawkesaspects fvutils \
< 	       fawkestf fawkesinterface fawkesblackboard \
< 	       fawkesgazeboaspect
---
> LIBS_gazsim_depthcam = m fawkescore fawkesutils fawkesaspects fvutils \
>                        fawkestf fawkesinterface fawkesblackboard \
>                        fawkesgazeboaspect
31a32
> PLUGINS_all = $(PLUGINDIR)/gazsim-depthcam.so
37c38,39
<   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) $(LDFLAGS_PCL) -lm $(call boost-libs-ldflags,system) -lboost_system
---
>   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) $(LDFLAGS_PCL) \
>              $(call boost-libs-ldflags,system)
39c41
<   PLUGINS_all = $(PLUGINDIR)/gazsim-depthcam.so
---
>   PLUGINS_build = $(PLUGINS_all)
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-laser/gazsim_laser_plugin.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-laser/gazsim_laser_plugin.cpp
21,22d20
< #include <core/plugin.h>
< 
24a23,24
> #include <core/plugin.h>
> 
34,36c34,35
<  public:
< 
<   /** Constructor.
---
> public:
> 	/** Constructor.
39,43c38,41
<   GazsimLaserPlugin(Configuration *config)
<     : Plugin(config)
<   {
<     thread_list.push_back(new LaserSimThread());
<   }
---
> 	explicit GazsimLaserPlugin(Configuration *config) : Plugin(config)
> 	{
> 		thread_list.push_back(new LaserSimThread());
> 	}
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-laser/gazsim_laser_thread.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-laser/gazsim_laser_thread.cpp
24,25c24
< #include <tf/types.h>
< #include <utils/math/angle.h>
---
> #include <aspect/logging.h>
27d25
< 
28a27,28
> #include <tf/types.h>
> #include <utils/math/angle.h>
30c30,31
< #include <gazebo/transport/Node.hh>
---
> #include <cmath>
> #include <cstdio>
31a33
> #include <gazebo/transport/Node.hh>
33,36d34
< #include <aspect/logging.h>
< 
< #include <cstdio>
< #include <cmath>
48,49c46,47
<   : Thread("LaserSimThread", Thread::OPMODE_WAITFORWAKEUP),
<     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)
---
> : Thread("LaserSimThread", Thread::OPMODE_WAITFORWAKEUP),
>   BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)
53c51,52
< void LaserSimThread::init()
---
> void
> LaserSimThread::init()
55c54
<   logger->log_debug(name(), "Initializing Simulation of the Laser Sensor");
---
> 	logger->log_debug(name(), "Initializing Simulation of the Laser Sensor");
57,73c56,72
<   //read config values
<   max_range_ = config->get_float("/gazsim/laser/max_range");
<   laser_topic_ = config->get_string("/gazsim/topics/laser");
<   interface_id_ = config->get_string("/gazsim/laser/interface-id");
<   frame_id_ = config->get_string("/gazsim/laser/frame-id");
< 
<   //open interface
<   laser_if_ = blackboard->open_for_writing<Laser360Interface>(interface_id_.c_str());
<   laser_if_->set_auto_timestamping(false);
< 
<   //subscribing to gazebo publisher
<   laser_sub_ = gazebonode->Subscribe(laser_topic_, &LaserSimThread::on_laser_data_msg, this);
< 
<   //initialize laser data
<   laser_data_ = (float *)malloc(sizeof(float) * 360);
<   laser_time_ = new Time(clock);
<   new_data_ = false;
---
> 	//read config values
> 	max_range_    = config->get_float("/gazsim/laser/max_range");
> 	laser_topic_  = config->get_string("/gazsim/topics/laser");
> 	interface_id_ = config->get_string("/gazsim/laser/interface-id");
> 	frame_id_     = config->get_string("/gazsim/laser/frame-id");
> 
> 	//open interface
> 	laser_if_ = blackboard->open_for_writing<Laser360Interface>(interface_id_.c_str());
> 	laser_if_->set_auto_timestamping(false);
> 
> 	//subscribing to gazebo publisher
> 	laser_sub_ = gazebonode->Subscribe(laser_topic_, &LaserSimThread::on_laser_data_msg, this);
> 
> 	//initialize laser data
> 	laser_data_ = (float *)malloc(sizeof(float) * 360);
> 	laser_time_ = new Time(clock);
> 	new_data_   = false;
75,76c74,75
<   //set frame in the interface
<   laser_if_->set_frame(frame_id_.c_str());
---
> 	//set frame in the interface
> 	laser_if_->set_frame(frame_id_.c_str());
79c78,79
< void LaserSimThread::finalize()
---
> void
> LaserSimThread::finalize()
81,83c81,83
<   blackboard->close(laser_if_);
<   free(laser_data_);
<   delete laser_time_;
---
> 	blackboard->close(laser_if_);
> 	free(laser_data_);
> 	delete laser_time_;
86c86,87
< void LaserSimThread::loop()
---
> void
> LaserSimThread::loop()
88,93c89,93
<   if(new_data_)
<   {
<     //write interface
<     laser_if_->set_distances(laser_data_);
<     laser_if_->set_timestamp(laser_time_);
<     laser_if_->write();
---
> 	if (new_data_) {
> 		//write interface
> 		laser_if_->set_distances(laser_data_);
> 		laser_if_->set_timestamp(laser_time_);
> 		laser_if_->write();
95,96c95,96
<     new_data_ = false;
<   }  
---
> 		new_data_ = false;
> 	}
99c99,100
< void LaserSimThread::on_laser_data_msg(ConstLaserScanStampedPtr &msg)
---
> void
> LaserSimThread::on_laser_data_msg(ConstLaserScanStampedPtr &msg)
101c102,106
<   //logger->log_info(name(), "Got new Laser data.\n");
---
> 	//logger->log_info(name(), "Got new Laser data.\n");
> 
> 	MutexLocker lock(loop_mutex);
> 
> 	const gazebo::msgs::LaserScan &scan = msg->scan();
103c108,109
<   MutexLocker lock(loop_mutex);
---
> 	//calculate start angle
> 	int start_index = (scan.angle_min() + 2 * M_PI) / M_PI * 180;
105c111
<   const gazebo::msgs::LaserScan &scan = msg->scan();
---
> 	int number_beams = scan.ranges_size();
107,125c113
<   //calculate start angle
<   int start_index = (scan.angle_min() + 2* M_PI) / M_PI * 180;
<   
<   int number_beams = scan.ranges_size();
< 
<   *laser_time_ = clock->now();
<   
<   //copy laser data
<   for(int i = 0; i < number_beams; i++)
<   {
<     const float range = scan.ranges(i);
<     if(range < max_range_)
<     {
<       laser_data_[(start_index + i) % 360] = range; 
<     }
<     else
<     {
<       laser_data_[(start_index + i) % 360] = NAN;
<     }
---
> 	*laser_time_ = clock->now();
127,128c115,124
<   }
<   new_data_  = true;
---
> 	//copy laser data
> 	for (int i = 0; i < number_beams; i++) {
> 		const float range = scan.ranges(i);
> 		if (range < max_range_) {
> 			laser_data_[(start_index + i) % 360] = range;
> 		} else {
> 			laser_data_[(start_index + i) % 360] = NAN;
> 		}
> 	}
> 	new_data_ = true;
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-laser/gazsim_laser_thread.h fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-laser/gazsim_laser_thread.h
21,22c21,22
< #ifndef __PLUGINS_GAZSIM_LASER_THREAD_H_
< #define __PLUGINS_GAZSIM_LASER_THREAD_H_
---
> #ifndef _PLUGINS_GAZSIM_LASER_THREAD_H_
> #define _PLUGINS_GAZSIM_LASER_THREAD_H_
24c24,25
< #include <core/threading/thread.h>
---
> #include <aspect/blackboard.h>
> #include <aspect/blocked_timing.h>
28,29c29
< #include <aspect/blackboard.h>
< #include <aspect/blocked_timing.h>
---
> #include <core/threading/thread.h>
33d32
< #include <gazebo/transport/TransportTypes.hh>
34a34
> #include <gazebo/transport/TransportTypes.hh>
37d36
< 
39,50c38,48
<   class Laser360Interface;
<   class Time;
< }
< 
< class LaserSimThread
< : public fawkes::Thread,
<   public fawkes::ClockAspect,
<   public fawkes::LoggingAspect,
<   public fawkes::ConfigurableAspect,
<   public fawkes::BlackBoardAspect,
<   public fawkes::BlockedTimingAspect,
<   public fawkes::GazeboAspect
---
> class Laser360Interface;
> class Time;
> } // namespace fawkes
> 
> class LaserSimThread : public fawkes::Thread,
>                        public fawkes::ClockAspect,
>                        public fawkes::LoggingAspect,
>                        public fawkes::ConfigurableAspect,
>                        public fawkes::BlackBoardAspect,
>                        public fawkes::BlockedTimingAspect,
>                        public fawkes::GazeboAspect
52,53c50,51
<  public:
<   LaserSimThread();
---
> public:
> 	LaserSimThread();
55,57c53,55
<   virtual void init();
<   virtual void loop();
<   virtual void finalize();
---
> 	virtual void init();
> 	virtual void loop();
> 	virtual void finalize();
59,62c57,60
<  private:
<   ///Subscriber to receive laser data from gazebo
<   gazebo::transport::SubscriberPtr laser_sub_;
<   std::string laser_topic_;
---
> private:
> 	///Subscriber to receive laser data from gazebo
> 	gazebo::transport::SubscriberPtr laser_sub_;
> 	std::string                      laser_topic_;
64,65c62,63
<   ///provided interface
<   fawkes::Laser360Interface *laser_if_;
---
> 	///provided interface
> 	fawkes::Laser360Interface *laser_if_;
67,69c65,67
<   ///storage for laser data
<   float        *laser_data_;
<   fawkes::Time *laser_time_;
---
> 	///storage for laser data
> 	float *       laser_data_;
> 	fawkes::Time *laser_time_;
71,72c69,70
<   ///is there new information to write in the interface?
<   bool new_data_;
---
> 	///is there new information to write in the interface?
> 	bool new_data_;
74,75c72,73
<   ///handler function for incoming laser data messages
<   void on_laser_data_msg(ConstLaserScanStampedPtr &msg);
---
> 	///handler function for incoming laser data messages
> 	void on_laser_data_msg(ConstLaserScanStampedPtr &msg);
77,78c75,76
<   ///maximal range of the laser sensor
<   float max_range_;  
---
> 	///maximal range of the laser sensor
> 	float max_range_;
80,81c78,79
<   std::string interface_id_;
<   std::string frame_id_;
---
> 	std::string interface_id_;
> 	std::string frame_id_;
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-laser/Makefile fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-laser/Makefile
31a32
> PLUGINS_all = $(PLUGINDIR)/gazsim-laser.so
39c40
<   PLUGINS_all = $(PLUGINDIR)/gazsim-laser.so
---
>   PLUGINS_build = $(PLUGINS_all)
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-localization/gazebo-plugin/gps.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-localization/gazebo-plugin/gps.cpp
21,22d20
< #include <math.h>
< 
24a23,26
> #include <utils/misc/gazebo_api_wrappers.h>
> 
> #include <math.h>
> 
37c39
<   printf("Destructing Gps Plugin!\n");
---
> 	printf("Destructing Gps Plugin!\n");
43c45,46
< void Gps::Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) 
---
> void
> Gps::Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
45,46c48,49
<   // Store the pointer to the model
<   this->model_ = _parent;
---
> 	// Store the pointer to the model
> 	this->model_ = _parent;
48,50c51,65
<   //get the model-name
<   this->name_ = model_->GetName();
<   printf("Loading Gps Plugin of model %s\n", name_.c_str());
---
> 	//get the model-name
> 	this->name_ = model_->GetName();
> 	printf("Loading Gps Plugin of model %s\n", name_.c_str());
> 
> 	// Listen to the update event. This event is broadcast every
> 	// simulation iteration.
> 	this->update_connection_ =
> 	  event::Events::ConnectWorldUpdateBegin(boost::bind(&Gps::OnUpdate, this, _1));
> 
> 	//Create the communication Node for communication with fawkes
> 	this->node_ = transport::NodePtr(new transport::Node());
> 
> 	//set namespace to the model name & init last sent time
> 	this->node_->Init(model_->GetWorld()->GZWRAP_NAME() + "/" + name_);
> 	last_sent_time_ = model_->GetWorld()->GZWRAP_SIM_TIME().Double();
52,65c67,68
<   // Listen to the update event. This event is broadcast every
<   // simulation iteration.
<   this->update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&Gps::OnUpdate, this, _1));
< 
<   //Create the communication Node for communication with fawkes
<   this->node_ = transport::NodePtr(new transport::Node());
<   //the namespace is set to the model name!
<   this->node_->Init(model_->GetWorld()->GetName()+"/"+name_);
< 
<   //init last sent time
<   last_sent_time_ = model_->GetWorld()->GetSimTime().Double();
< 
<   //create publisher
<   this->gps_pub_ = this->node_->Advertise<msgs::Pose>("~/gazsim/gps/");
---
> 	//create publisher
> 	this->gps_pub_ = this->node_->Advertise<msgs::Pose>("~/gazsim/gps/");
68d70
< 
71c73,74
< void Gps::OnUpdate(const common::UpdateInfo & /*_info*/)
---
> void
> Gps::OnUpdate(const common::UpdateInfo & /*_info*/)
73,79c76,82
<   //Send position information to Fawkes
<   double time = model_->GetWorld()->GetSimTime().Double();
<   if(time - last_sent_time_ > (1.0 / 10.0))
<   {
<     last_sent_time_ = time;
<     send_position();
<   }
---
> 	//Send position information to Fawkes
> 	double time = model_->GetWorld()->GZWRAP_SIM_TIME().Double();
> 
> 	if (time - last_sent_time_ > (1.0 / 10.0)) {
> 		last_sent_time_ = time;
> 		send_position();
> 	}
84c87,88
< void Gps::Reset()
---
> void
> Gps::Reset()
91c95,96
< void Gps::send_position()
---
> void
> Gps::send_position()
93,107c98,111
<   if(gps_pub_->HasConnections())
<   {
<     //build message
<     msgs::Pose posMsg;
<     posMsg.mutable_position()->set_x(this->model_->GetWorldPose().pos.x);
<     posMsg.mutable_position()->set_y(this->model_->GetWorldPose().pos.y);
<     posMsg.mutable_position()->set_z(this->model_->GetWorldPose().pos.z);
<     posMsg.mutable_orientation()->set_x(this->model_->GetWorldPose().rot.x);
<     posMsg.mutable_orientation()->set_y(this->model_->GetWorldPose().rot.y);
<     posMsg.mutable_orientation()->set_z(this->model_->GetWorldPose().rot.z);
<     posMsg.mutable_orientation()->set_w(this->model_->GetWorldPose().rot.w);
< 
<     //send
<     gps_pub_->Publish(posMsg);
<   }
---
> 	if (gps_pub_->HasConnections()) {
> 		//build message
> 		msgs::Pose posMsg;
> 		posMsg.mutable_position()->set_x(this->model_->GZWRAP_WORLD_POSE().GZWRAP_POS_X);
> 		posMsg.mutable_position()->set_y(this->model_->GZWRAP_WORLD_POSE().GZWRAP_POS_Y);
> 		posMsg.mutable_position()->set_z(this->model_->GZWRAP_WORLD_POSE().GZWRAP_POS_Z);
> 		posMsg.mutable_orientation()->set_x(this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_X);
> 		posMsg.mutable_orientation()->set_y(this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_Y);
> 		posMsg.mutable_orientation()->set_z(this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_Z);
> 		posMsg.mutable_orientation()->set_w(this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_W);
> 
> 		//send
> 		gps_pub_->Publish(posMsg);
> 	}
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-localization/gazebo-plugin/gps.h fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-localization/gazebo-plugin/gps.h
21a22
> #include <gazebo/common/common.hh>
24,25d24
< #include <gazebo/common/common.hh>
< #include <stdio.h>
27a27
> #include <stdio.h>
30,32c30,31
< namespace gazebo
< {
<   /**
---
> namespace gazebo {
> /**
36,67c35,66
<   class Gps : public ModelPlugin
<   {
<   public:
<     Gps();
<    ~Gps();
< 
<     //Overridden ModelPlugin-Functions
<     virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/);
<     virtual void OnUpdate(const common::UpdateInfo &);
<     virtual void Reset();
< 
<   private:
<     /// Pointer to the gazbeo model
<     physics::ModelPtr model_;
<     /// Pointer to the update event connection
<     event::ConnectionPtr update_connection_;
<     ///Node for communication to fawkes
<     transport::NodePtr node_;
<     ///name of the gps and the communication channel
<     std::string name_;
< 
<     ///time variable to send in intervals
<     double last_sent_time_;
< 
<     //Gps Stuff:
<     ///Functions for sending information to fawkes:
<     void send_position();
< 
<     ///Publisher for GyroAngle
<     transport::PublisherPtr gps_pub_;
<   };
< }
---
> class Gps : public ModelPlugin
> {
> public:
> 	Gps();
> 	~Gps();
> 
> 	//Overridden ModelPlugin-Functions
> 	virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/);
> 	virtual void OnUpdate(const common::UpdateInfo &);
> 	virtual void Reset();
> 
> private:
> 	/// Pointer to the gazbeo model
> 	physics::ModelPtr model_;
> 	/// Pointer to the update event connection
> 	event::ConnectionPtr update_connection_;
> 	///Node for communication to fawkes
> 	transport::NodePtr node_;
> 	///name of the gps and the communication channel
> 	std::string name_;
> 
> 	///time variable to send in intervals
> 	double last_sent_time_;
> 
> 	//Gps Stuff:
> 	///Functions for sending information to fawkes:
> 	void send_position();
> 
> 	///Publisher for GyroAngle
> 	transport::PublisherPtr gps_pub_;
> };
> } // namespace gazebo
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-localization/gazebo-plugin/Makefile fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-localization/gazebo-plugin/Makefile
26c26
< LIBS_gazebo_libgps = gazsim_msgs
---
> LIBS_gazebo_libgps = m gazsim_msgs
32c32,33
< OBJS_all    = $(OBJS_gazebo_libgps)
---
> OBJS_all = $(OBJS_gazebo_libgps)
> LIBS_all = $(LIBDIR)/gazebo/libgps.so
36c37,38
<   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) -lm $(call boost-libs-ldflags,system) -lboost_system
---
>   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) \
>              $(call boost-libs-ldflags,system)
38c40
<   LIBS_all = $(LIBDIR)/gazebo/libgps.so
---
>   LIBS_build = $(LIBS_all)
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-localization/gazsim_localization_plugin.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-localization/gazsim_localization_plugin.cpp
22,23d21
< #include <core/plugin.h>
< 
25a24,25
> #include <core/plugin.h>
> 
35,37c35,36
<  public:
< 
<   /** Constructor.
---
> public:
> 	/** Constructor.
40,44c39,42
<   GazsimLocalizationPlugin(Configuration *config)
<     : Plugin(config)
<   {
<     thread_list.push_back(new LocalizationSimThread());
<   }
---
> 	explicit GazsimLocalizationPlugin(Configuration *config) : Plugin(config)
> 	{
> 		thread_list.push_back(new LocalizationSimThread());
> 	}
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-localization/gazsim_localization_thread.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-localization/gazsim_localization_thread.cpp
24,27c24
< #include <tf/types.h>
< #include <stdio.h>
< #include <math.h>
< #include <utils/math/angle.h>
---
> #include <aspect/logging.h>
29d25
< 
30a27,30
> #include <tf/transform_publisher.h>
> #include <tf/types.h>
> #include <utils/math/angle.h>
> #include <utils/time/clock.h>
32d31
< #include <gazebo/transport/Node.hh>
33a33
> #include <gazebo/transport/Node.hh>
35,37c35,36
< #include <aspect/logging.h>
< #include <tf/transform_publisher.h>
< #include <utils/time/clock.h>
---
> #include <math.h>
> #include <stdio.h>
49,51c48,50
<   : Thread("LocalizationSimThread", Thread::OPMODE_WAITFORWAKEUP),
<     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS),
<     TransformAspect(TransformAspect::BOTH, "Pose")
---
> : Thread("LocalizationSimThread", Thread::OPMODE_WAITFORWAKEUP),
>   BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS),
>   TransformAspect(TransformAspect::BOTH, "Pose")
55c54,55
< void LocalizationSimThread::init()
---
> void
> LocalizationSimThread::init()
57,60c57
<   logger->log_debug(name(), "Initializing Simulation of the Localization");
< 
<   //open interface
<   localization_if_ = blackboard->open_for_writing<Position3DInterface>("Pose");
---
> 	logger->log_debug(name(), "Initializing Simulation of the Localization");
62,66c59,60
<   //read cofig values
<   gps_topic_ = config->get_string("/gazsim/topics/gps");
<   transform_tolerance_ = config->get_float("/plugins/amcl/transform_tolerance");
<   global_frame_id_ = config->get_string("/plugins/amcl/global_frame_id");
<   odom_frame_id_ = config->get_string("/plugins/amcl/odom_frame_id");
---
> 	//open interface
> 	localization_if_ = blackboard->open_for_writing<Position3DInterface>("Pose");
68,69c62,70
<   //subscribing to gazebo publisher
<   localization_sub_ = gazebonode->Subscribe(gps_topic_, &LocalizationSimThread::on_localization_msg, this);
---
> 	//read cofig values
> 	gps_topic_           = config->get_string("/gazsim/topics/gps");
> 	transform_tolerance_ = config->get_float("/plugins/amcl/transform_tolerance");
> 	global_frame_id_     = config->get_string("/plugins/amcl/global_frame_id");
> 	odom_frame_id_       = config->get_string("/plugins/amcl/odom_frame_id");
> 
> 	//subscribing to gazebo publisher
> 	localization_sub_ =
> 	  gazebonode->Subscribe(gps_topic_, &LocalizationSimThread::on_localization_msg, this);
71c72
<   new_data_ = false;
---
> 	new_data_ = false;
74c75,76
< void LocalizationSimThread::finalize()
---
> void
> LocalizationSimThread::finalize()
76c78
<   blackboard->close(localization_if_);
---
> 	blackboard->close(localization_if_);
79c81,82
< void LocalizationSimThread::loop()
---
> void
> LocalizationSimThread::loop()
81,102c84,105
<   if(new_data_)
<   {
<     //write interface
<     localization_if_->set_frame(global_frame_id_.c_str());
<     localization_if_->set_visibility_history(100);
<     localization_if_->set_translation(0, x_);
<     localization_if_->set_translation(1, y_);
<     localization_if_->set_translation(2, z_);
<     localization_if_->set_rotation(0, quat_x_);
<     localization_if_->set_rotation(1, quat_y_);
<     localization_if_->set_rotation(2, quat_z_);
<     localization_if_->set_rotation(3, quat_w_);
<     localization_if_->write();
< 
<     //publish transform (similar as in amcl_thread.cpp)
<     tf::Quaternion rotation = tf::Quaternion(quat_x_, quat_y_, quat_z_, quat_w_);
<     tf::Point position = tf::Point(x_, y_, z_);
<     tf::Transform latest_tf_ = tf::Transform(rotation,position);
<     Time transform_expiration =
<       (clock->now() + transform_tolerance_);
<     tf_publisher->send_transform(latest_tf_, transform_expiration,
< 				 global_frame_id_, odom_frame_id_);
---
> 	if (new_data_) {
> 		//write interface
> 		localization_if_->set_frame(global_frame_id_.c_str());
> 		localization_if_->set_visibility_history(100);
> 		localization_if_->set_translation(0, x_);
> 		localization_if_->set_translation(1, y_);
> 		localization_if_->set_translation(2, z_);
> 		localization_if_->set_rotation(0, quat_x_);
> 		localization_if_->set_rotation(1, quat_y_);
> 		localization_if_->set_rotation(2, quat_z_);
> 		localization_if_->set_rotation(3, quat_w_);
> 		localization_if_->write();
> 
> 		//publish transform (similar as in amcl_thread.cpp)
> 		tf::Quaternion rotation             = tf::Quaternion(quat_x_, quat_y_, quat_z_, quat_w_);
> 		tf::Point      position             = tf::Point(x_, y_, z_);
> 		tf::Transform  latest_tf_           = tf::Transform(rotation, position);
> 		Time           transform_expiration = (clock->now() + transform_tolerance_);
> 		tf_publisher->send_transform(latest_tf_,
> 		                             transform_expiration,
> 		                             global_frame_id_,
> 		                             odom_frame_id_);
104,105c107,108
<     new_data_ = false;
<   }
---
> 		new_data_ = false;
> 	}
108c111,112
< void LocalizationSimThread::on_localization_msg(ConstPosePtr &msg)
---
> void
> LocalizationSimThread::on_localization_msg(ConstPosePtr &msg)
110,123c114,127
<   //logger->log_info(name(), "Got new Localization data.\n");
<   
<   MutexLocker lock(loop_mutex);
< 
<   //read data from message
<   x_ = msg->position().x();
<   y_ = msg->position().y();
<   z_ = msg->position().z();
<   quat_x_ = msg->orientation().x();
<   quat_y_ = msg->orientation().y();
<   quat_z_ = msg->orientation().z();
<   quat_w_ = msg->orientation().w();
<   
<   new_data_ = true;
---
> 	//logger->log_info(name(), "Got new Localization data.\n");
> 
> 	MutexLocker lock(loop_mutex);
> 
> 	//read data from message
> 	x_      = msg->position().x();
> 	y_      = msg->position().y();
> 	z_      = msg->position().z();
> 	quat_x_ = msg->orientation().x();
> 	quat_y_ = msg->orientation().y();
> 	quat_z_ = msg->orientation().z();
> 	quat_w_ = msg->orientation().w();
> 
> 	new_data_ = true;
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-localization/gazsim_localization_thread.h fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-localization/gazsim_localization_thread.h
22,23c22,23
< #ifndef __PLUGINS_GAZSIM_LOCALIZATION_THREAD_H_
< #define __PLUGINS_GAZSIM_LOCALIZATION_THREAD_H_
---
> #ifndef _PLUGINS_GAZSIM_LOCALIZATION_THREAD_H_
> #define _PLUGINS_GAZSIM_LOCALIZATION_THREAD_H_
25c25,26
< #include <core/threading/thread.h>
---
> #include <aspect/blackboard.h>
> #include <aspect/blocked_timing.h>
29,30d29
< #include <aspect/blackboard.h>
< #include <aspect/blocked_timing.h>
31a31
> #include <core/threading/thread.h>
35d34
< #include <gazebo/transport/TransportTypes.hh>
36a36
> #include <gazebo/transport/TransportTypes.hh>
39d38
< 
41c40
<   class Position3DInterface;
---
> class Position3DInterface;
44,52c43,50
< class LocalizationSimThread
< : public fawkes::Thread,
<   public fawkes::ClockAspect,
<   public fawkes::LoggingAspect,
<   public fawkes::ConfigurableAspect,
<   public fawkes::BlackBoardAspect,
<   public fawkes::BlockedTimingAspect,
<   public fawkes::GazeboAspect,
<   public fawkes::TransformAspect
---
> class LocalizationSimThread : public fawkes::Thread,
>                               public fawkes::ClockAspect,
>                               public fawkes::LoggingAspect,
>                               public fawkes::ConfigurableAspect,
>                               public fawkes::BlackBoardAspect,
>                               public fawkes::BlockedTimingAspect,
>                               public fawkes::GazeboAspect,
>                               public fawkes::TransformAspect
54,55c52,53
<  public:
<   LocalizationSimThread();
---
> public:
> 	LocalizationSimThread();
57,89c55,87
<   virtual void init();
<   virtual void loop();
<   virtual void finalize();
< 
<  private:
<   //Subscriber to receive localization data from gazebo
<   gazebo::transport::SubscriberPtr localization_sub_;
<   std::string gps_topic_;
< 
<   //provided interface
<   fawkes::Position3DInterface *localization_if_;
< 
<   //handler function for incoming localization data messages
<   void on_localization_msg(ConstPosePtr &msg);
< 
<   //is there new information to write in the interface?
<   bool new_data_;
< 
<   //localization data
<   double x_;
<   double y_;
<   double z_;
<   double quat_x_;
<   double quat_y_;
<   double quat_z_;
<   double quat_w_;
< 
<   //time the transform should be up to date
<   double transform_tolerance_;
< 
<   //frame ids for transform
<   std::string odom_frame_id_;
<   std::string global_frame_id_;
---
> 	virtual void init();
> 	virtual void loop();
> 	virtual void finalize();
> 
> private:
> 	//Subscriber to receive localization data from gazebo
> 	gazebo::transport::SubscriberPtr localization_sub_;
> 	std::string                      gps_topic_;
> 
> 	//provided interface
> 	fawkes::Position3DInterface *localization_if_;
> 
> 	//handler function for incoming localization data messages
> 	void on_localization_msg(ConstPosePtr &msg);
> 
> 	//is there new information to write in the interface?
> 	bool new_data_;
> 
> 	//localization data
> 	double x_;
> 	double y_;
> 	double z_;
> 	double quat_x_;
> 	double quat_y_;
> 	double quat_z_;
> 	double quat_w_;
> 
> 	//time the transform should be up to date
> 	double transform_tolerance_;
> 
> 	//frame ids for transform
> 	std::string odom_frame_id_;
> 	std::string global_frame_id_;
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-localization/Makefile fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-localization/Makefile
33a34
> PLUGINS_all = $(PLUGINDIR)/gazsim-localization.so
41c42
<   PLUGINS_all = $(PLUGINDIR)/gazsim-localization.so
---
>   PLUGINS_build = $(PLUGINS_all)
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-robotino/gazebo-plugin-gyro/gyro.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-robotino/gazebo-plugin-gyro/gyro.cpp
21,22d20
< #include <math.h>
< 
24a23,26
> #include <utils/misc/gazebo_api_wrappers.h>
> 
> #include <math.h>
> 
30c32
< Gyro::Gyro()
---
> Gyro::Gyro() : last_sent_time_(0.0), send_interval_(0.0)
36c38
<   printf("Destructing Gyro Plugin!\n");
---
> 	printf("Destructing Gyro Plugin!\n");
42c44,45
< void Gyro::Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) 
---
> void
> Gyro::Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
44,62c47,48
<   // Store the pointer to the model
<   this->model_ = _parent;
< 
<   //get the model-name
<   this->name_ = model_->GetName();
<   printf("Loading Gyro Plugin of model %s\n", name_.c_str());
< 
<   // Listen to the update event. This event is broadcast every
<   // simulation iteration.
<   this->update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&Gyro::OnUpdate, this, _1));
< 
<   //Create the communication Node for communication with fawkes
<   this->node_ = transport::NodePtr(new transport::Node());
<   //the namespace is set to the model name!
<   this->node_->Init(model_->GetWorld()->GetName()+"/"+name_);
< 
< 
<   //create publisher
<   this->gyro_pub_ = this->node_->Advertise<msgs::Vector3d>("~/RobotinoSim/Gyro/");
---
> 	// Store the pointer to the model
> 	this->model_ = _parent;
64,66c50,69
<   //init last sent time
<   last_sent_time_ = model_->GetWorld()->GetSimTime().Double();
<   this->send_interval_ = 0.05;
---
> 	//get the model-name
> 	this->name_ = model_->GetName();
> 	printf("Loading Gyro Plugin of model %s\n", name_.c_str());
> 
> 	// Listen to the update event. This event is broadcast every
> 	// simulation iteration.
> 	this->update_connection_ =
> 	  event::Events::ConnectWorldUpdateBegin(boost::bind(&Gyro::OnUpdate, this, _1));
> 
> 	//Create the communication Node for communication with fawkes
> 	this->node_ = transport::NodePtr(new transport::Node());
> 	//the namespace is set to the model name!
> 	this->node_->Init(model_->GetWorld()->GZWRAP_NAME() + "/" + name_);
> 
> 	//create publisher
> 	this->gyro_pub_ = this->node_->Advertise<msgs::Vector3d>("~/RobotinoSim/Gyro/");
> 
> 	//init last sent time
> 	last_sent_time_      = model_->GetWorld()->GZWRAP_SIM_TIME().Double();
> 	this->send_interval_ = 0.05;
71c74,75
< void Gyro::OnUpdate(const common::UpdateInfo & /*_info*/)
---
> void
> Gyro::OnUpdate(const common::UpdateInfo & /*_info*/)
73,79c77,82
<   //Send gyro information to Fawkes
<   double time = model_->GetWorld()->GetSimTime().Double();
<   if(time - last_sent_time_ > send_interval_)
<   {
<     last_sent_time_ = time;
<     send_gyro();
<   }
---
> 	//Send gyro information to Fawkes
> 	double time = model_->GetWorld()->GZWRAP_SIM_TIME().Double();
> 	if (time - last_sent_time_ > send_interval_) {
> 		last_sent_time_ = time;
> 		send_gyro();
> 	}
84c87,88
< void Gyro::Reset()
---
> void
> Gyro::Reset()
88c92,93
< void Gyro::send_gyro()
---
> void
> Gyro::send_gyro()
90,105c95,109
<   if(gyro_pub_->HasConnections())
<   {
<     //Read gyro from simulation
<     float roll = this->model_->GetWorldPose().rot.GetAsEuler().x;
<     float pitch = this->model_->GetWorldPose().rot.GetAsEuler().y;
<     float yaw = this->model_->GetWorldPose().rot.GetAsEuler().z;
< 
<     //build message
<     msgs::Vector3d gyroMsg;
<     gyroMsg.set_x(roll);
<     gyroMsg.set_y(pitch);
<     gyroMsg.set_z(yaw);
< 
<     //send
<     gyro_pub_->Publish(gyroMsg);
<   }
---
> 	if (gyro_pub_->HasConnections()) {
> 		//Read gyro from simulation
> 		float roll  = this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_EULER_X;
> 		float pitch = this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_EULER_Y;
> 		float yaw   = this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_EULER_Z;
> 
> 		//build message
> 		msgs::Vector3d gyroMsg;
> 		gyroMsg.set_x(roll);
> 		gyroMsg.set_y(pitch);
> 		gyroMsg.set_z(yaw);
> 
> 		//send
> 		gyro_pub_->Publish(gyroMsg);
> 	}
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-robotino/gazebo-plugin-gyro/gyro.h fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-robotino/gazebo-plugin-gyro/gyro.h
21a22
> #include <gazebo/common/common.hh>
24,25d24
< #include <gazebo/common/common.hh>
< #include <stdio.h>
27a27
> #include <stdio.h>
30,32c30,31
< namespace gazebo
< {   
<   /** @class Gyro
---
> namespace gazebo {
> /** @class Gyro
36,74c35,72
<   class Gyro : public ModelPlugin
<   {
<   public:
<     ///Constructor
<     Gyro();
< 
<     ///Destructor
<     ~Gyro();
< 
<     //Overridden ModelPlugin-Functions
<     virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/);
<     virtual void OnUpdate(const common::UpdateInfo &);
<     virtual void Reset();
< 
<   private:
<     /// Pointer to the model
<     physics::ModelPtr model_;
<     /// Pointer to the update event connection
<     event::ConnectionPtr update_connection_;
<     ///Node for communication to fawkes
<     transport::NodePtr node_;
<     ///name of the gyro and the communication channel
<     std::string name_;
< 
<     ///time variable to send in intervals
<     double last_sent_time_;
< 
<     ///time interval between to gyro msgs
<     double send_interval_;
< 
< 
<     //Gyro Stuff:
<     ///Sending Gyro-angle to fawkes:
<     void send_gyro();
< 
<     ///Publisher for GyroAngle
<     transport::PublisherPtr gyro_pub_;  
<   };
< }
---
> class Gyro : public ModelPlugin
> {
> public:
> 	///Constructor
> 	Gyro();
> 
> 	///Destructor
> 	~Gyro();
> 
> 	//Overridden ModelPlugin-Functions
> 	virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/);
> 	virtual void OnUpdate(const common::UpdateInfo &);
> 	virtual void Reset();
> 
> private:
> 	/// Pointer to the model
> 	physics::ModelPtr model_;
> 	/// Pointer to the update event connection
> 	event::ConnectionPtr update_connection_;
> 	///Node for communication to fawkes
> 	transport::NodePtr node_;
> 	///name of the gyro and the communication channel
> 	std::string name_;
> 
> 	///time variable to send in intervals
> 	double last_sent_time_;
> 
> 	///time interval between to gyro msgs
> 	double send_interval_;
> 
> 	//Gyro Stuff:
> 	///Sending Gyro-angle to fawkes:
> 	void send_gyro();
> 
> 	///Publisher for GyroAngle
> 	transport::PublisherPtr gyro_pub_;
> };
> } // namespace gazebo
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-robotino/gazebo-plugin-gyro/Makefile fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-robotino/gazebo-plugin-gyro/Makefile
26c26
< LIBS_gazebo_libgyro = gazsim_msgs
---
> LIBS_gazebo_libgyro = m gazsim_msgs
32c32,33
< OBJS_all    = $(OBJS_gazebo_libgyro)
---
> OBJS_all = $(OBJS_gazebo_libgyro)
> LIBS_all = $(LIBDIR)/gazebo/libgyro.so
36c37,38
<   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) -lm $(call boost-libs-ldflags,system) -lboost_system
---
>   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) \
>              $(call boost-libs-ldflags,system)
38c40
<   LIBS_all = $(LIBDIR)/gazebo/libgyro.so
---
>   LIBS_build = $(LIBS_all)
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-robotino/gazebo-plugin-motor/Makefile fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-robotino/gazebo-plugin-motor/Makefile
26c26
< LIBS_gazebo_libmotor = gazsim_msgs
---
> LIBS_gazebo_libmotor = m gazsim_msgs
32c32,33
< OBJS_all    = $(OBJS_gazebo_libmotor)
---
> OBJS_all = $(OBJS_gazebo_libmotor)
> LIBS_all = $(LIBDIR)/gazebo/libmotor.so
36c37,38
<   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) -lm $(call boost-libs-ldflags,system) -lboost_system
---
>   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) \
>              $(call boost-libs-ldflags,system)
38c40
<   LIBS_all = $(LIBDIR)/gazebo/libmotor.so
---
>   LIBS_build = $(LIBS_all)
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-robotino/gazebo-plugin-motor/motor.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-robotino/gazebo-plugin-motor/motor.cpp
21,22d20
< #include <math.h>
< 
24a23,26
> #include <utils/misc/gazebo_api_wrappers.h>
> 
> #include <math.h>
> 
30c32
< Motor::Motor()
---
> Motor::Motor() : vx_(0), vy_(0), vomega_(0)
36c38
<   printf("Destructing Motor Plugin!\n");
---
> 	printf("Destructing Motor Plugin!\n");
42c44,45
< void Motor::Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/) 
---
> void
> Motor::Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
44,49c47,48
<   // Store the pointer to the model
<   this->model_ = _parent;
< 
<   //get the model-name
<   this->name_ = model_->GetName();
<   printf("Loading Motor Plugin of model %s\n", name_.c_str());
---
> 	// Store the pointer to the model
> 	this->model_ = _parent;
51,67c50,72
<   // Listen to the update event. This event is broadcast every
<   // simulation iteration.
<   this->update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&Motor::OnUpdate, this, _1));
< 
<   //Create the communication Node for communication with fawkes
<   this->node_ = transport::NodePtr(new transport::Node());
<   //the namespace is set to the model name!
<   this->node_->Init(model_->GetWorld()->GetName()+"/"+name_);
< 
< 
<   //initialize movement commands:
<   vx_ = 0.0;
<   vy_ = 0.0;
<   vomega_ = 0.0;
< 
<   //create subscriber
<   this->motor_move_sub_ = this->node_->Subscribe(std::string("~/RobotinoSim/MotorMove/"), &Motor::on_motor_move_msg, this);
---
> 	//get the model-name
> 	this->name_ = model_->GetName();
> 	printf("Loading Motor Plugin of model %s\n", name_.c_str());
> 
> 	// Listen to the update event. This event is broadcast every
> 	// simulation iteration.
> 	this->update_connection_ =
> 	  event::Events::ConnectWorldUpdateBegin(boost::bind(&Motor::OnUpdate, this, _1));
> 
> 	//Create the communication Node for communication with fawkes
> 	this->node_ = transport::NodePtr(new transport::Node());
> 	//the namespace is set to the model name!
> 	this->node_->Init(model_->GetWorld()->GZWRAP_NAME() + "/" + name_);
> 
> 	//initialize movement commands:
> 	vx_     = 0.0;
> 	vy_     = 0.0;
> 	vomega_ = 0.0;
> 
> 	//create subscriber
> 	this->motor_move_sub_ = this->node_->Subscribe(std::string("~/RobotinoSim/MotorMove/"),
> 	                                               &Motor::on_motor_move_msg,
> 	                                               this);
72c77,78
< void Motor::OnUpdate(const common::UpdateInfo & /*_info*/)
---
> void
> Motor::OnUpdate(const common::UpdateInfo & /*_info*/)
74,85c80,91
<   //Apply movement command
<   float x,y;
<   float yaw = this->model_->GetWorldPose().rot.GetAsEuler().z;
<   //foward part
<   x = cos(yaw) * vx_;
<   y = sin(yaw) * vx_;
<   //sideways part
<   x += cos(yaw + 3.1415926f / 2) * vy_;
<   y += sin(yaw + 3.1415926f / 2) * vy_;
<   // Apply velocity to the model.
<   this->model_->SetLinearVel(math::Vector3(x, y, 0));
<   this->model_->SetAngularVel(math::Vector3(0, 0, vomega_));
---
> 	//Apply movement command
> 	float x, y;
> 	float yaw = this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_EULER_Z;
> 	//foward part
> 	x = cos(yaw) * vx_;
> 	y = sin(yaw) * vx_;
> 	//sideways part
> 	x += cos(yaw + 3.1415926f / 2) * vy_;
> 	y += sin(yaw + 3.1415926f / 2) * vy_;
> 	// Apply velocity to the model.
> 	this->model_->SetLinearVel(gzwrap::Vector3d(x, y, 0));
> 	this->model_->SetAngularVel(gzwrap::Vector3d(0, 0, vomega_));
90c96,97
< void Motor::Reset()
---
> void
> Motor::Reset()
96,97c103,105
<  */ 
< void Motor::on_motor_move_msg(ConstVector3dPtr &msg)
---
>  */
> void
> Motor::on_motor_move_msg(ConstVector3dPtr &msg)
99,103c107,111
<   //printf("Got MotorMove Msg!!! %f %f %f\n", msg->x(), msg->y(), msg->z());
<   //Transform relative motion into ablosulte motion
<   vx_ = msg->x();
<   vy_ = msg->y();
<   vomega_ = msg->z();
---
> 	//printf("Got MotorMove Msg!!! %f %f %f\n", msg->x(), msg->y(), msg->z());
> 	//Transform relative motion into ablosulte motion
> 	vx_     = msg->x();
> 	vy_     = msg->y();
> 	vomega_ = msg->z();
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-robotino/gazebo-plugin-motor/motor.h fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-robotino/gazebo-plugin-motor/motor.h
21a22
> #include <gazebo/common/common.hh>
24,25d24
< #include <gazebo/common/common.hh>
< #include <stdio.h>
27a27
> #include <stdio.h>
30,32c30,31
< namespace gazebo
< {   
<   /** @class Motor
---
> namespace gazebo {
> /** @class Motor
36,73c35,70
<   class Motor : public ModelPlugin
<   {
<   public:
<     ///Constructor
<     Motor();
< 
<     ///Destructor
<     ~Motor();
< 
<     //Overridden ModelPlugin-Functions
<     virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/);
<     virtual void OnUpdate(const common::UpdateInfo &);
<     virtual void Reset();
< 
<   private:
<     /// Pointer to the Gazebo model
<     physics::ModelPtr model_;
<     /// Pointer to the update event connection
<     event::ConnectionPtr update_connection_;
<     ///Node for communication to fawkes
<     transport::NodePtr node_;
<     ///name of the motor and the communication channel
<     std::string name_;
< 
< 
<     //Motor Stuff:
<     void on_motor_move_msg(ConstVector3dPtr &msg);
< 
<     ///Suscriber for MotorMove Interfaces from Fawkes
<     transport::SubscriberPtr motor_move_sub_;
< 
< 
<     //current movement commands:
<     float vx_;
<     float vy_;
<     float vomega_;  
<   };
< }
---
> class Motor : public ModelPlugin
> {
> public:
> 	///Constructor
> 	Motor();
> 
> 	///Destructor
> 	~Motor();
> 
> 	//Overridden ModelPlugin-Functions
> 	virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/);
> 	virtual void OnUpdate(const common::UpdateInfo &);
> 	virtual void Reset();
> 
> private:
> 	/// Pointer to the Gazebo model
> 	physics::ModelPtr model_;
> 	/// Pointer to the update event connection
> 	event::ConnectionPtr update_connection_;
> 	///Node for communication to fawkes
> 	transport::NodePtr node_;
> 	///name of the motor and the communication channel
> 	std::string name_;
> 
> 	//Motor Stuff:
> 	void on_motor_move_msg(ConstVector3dPtr &msg);
> 
> 	///Suscriber for MotorMove Interfaces from Fawkes
> 	transport::SubscriberPtr motor_move_sub_;
> 
> 	//current movement commands:
> 	float vx_;
> 	float vy_;
> 	float vomega_;
> };
> } // namespace gazebo
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-robotino/gazsim_robotino_plugin.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-robotino/gazsim_robotino_plugin.cpp
21,22d20
< #include <core/plugin.h>
< 
24a23,24
> #include <core/plugin.h>
> 
34,36c34,35
<  public:
< 
<   /** Constructor.
---
> public:
> 	/** Constructor.
39,43c38,41
<   GazsimRobotinoPlugin(Configuration *config)
<     : Plugin(config)
<   {
<     thread_list.push_back(new RobotinoSimThread());
<   }
---
> 	explicit GazsimRobotinoPlugin(Configuration *config) : Plugin(config)
> 	{
> 		thread_list.push_back(new RobotinoSimThread());
> 	}
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-robotino/gazsim_robotino_thread.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-robotino/gazsim_robotino_thread.cpp
23,28d22
< #include <tf/types.h>
< #include <core/threading/mutex_locker.h>
< 
< #include <gazebo/transport/Node.hh>
< #include <gazebo/msgs/msgs.hh>
< #include <gazebo/transport/transport.hh>
30c24,25
< 
---
> #include <core/threading/mutex_locker.h>
> #include <interfaces/IMUInterface.h>
34,35d28
< #include <interfaces/IMUInterface.h>
< 
37c30
< #include <utils/time/clock.h>
---
> #include <tf/types.h>
38a32
> #include <utils/time/clock.h>
40a35,37
> #include <gazebo/msgs/msgs.hh>
> #include <gazebo/transport/Node.hh>
> #include <gazebo/transport/transport.hh>
55,57c52,54
<   : Thread("RobotinoSimThread", Thread::OPMODE_WAITFORWAKEUP),
<     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE), //sonsor and act here
<     TransformAspect(TransformAspect::DEFER_PUBLISHER)
---
> : Thread("RobotinoSimThread", Thread::OPMODE_WAITFORWAKEUP),
>   BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE), //sonsor and act here
>   TransformAspect(TransformAspect::DEFER_PUBLISHER)
64,146c61,62
<   //get a connection to gazebo (copied from gazeboscene)
<   logger->log_debug(name(), "Creating Gazebo publishers");
< 
<   //read config values
<   cfg_frame_odom_ = config->get_string("/frames/odom");
<   cfg_frame_base_ = config->get_string("/frames/base");
<   slippery_wheels_enabled_ = config->get_bool("gazsim/robotino/motor/slippery-wheels-enabled");
<   slippery_wheels_threshold_ = config->get_float("gazsim/robotino/motor/slippery-wheels-threshold");
<   moving_speed_factor_ = config->get_float("gazsim/robotino/motor/moving-speed-factor");
<   rotation_speed_factor_ = config->get_float("gazsim/robotino/motor/rotation-speed-factor");
<   gripper_laser_threshold_ = config->get_float("/gazsim/robotino/gripper-laser-threshold");
<   gripper_laser_value_far_ = config->get_float("/gazsim/robotino/gripper-laser-value-far");
<   gripper_laser_value_near_ = config->get_float("/gazsim/robotino/gripper-laser-value-near");
<   have_gripper_sensors_ = config->exists("/hardware/robotino/sensors/right_ir_id")
<     && config->exists("/hardware/robotino/sensors/left_ir_id");
<   if(have_gripper_sensors_)
<   {
<     gripper_laser_right_pos_ = config->get_int("/hardware/robotino/sensors/right_ir_id");
<     gripper_laser_left_pos_ = config->get_int("/hardware/robotino/sensors/left_ir_id");
<   }
<   gyro_buffer_size_ = config->get_int("/gazsim/robotino/gyro-buffer-size");
<   gyro_delay_ = config->get_float("/gazsim/robotino/gyro-delay");
<   infrared_sensor_index_ = config->get_int("/gazsim/robotino/infrared-sensor-index");
< 
<   tf_enable_publisher(cfg_frame_base_.c_str());
<  
<   //Open interfaces
<   motor_if_  = blackboard->open_for_writing<MotorInterface>("Robotino");
<   switch_if_ = blackboard->open_for_writing<fawkes::SwitchInterface>("Robotino Motor");
<   sens_if_   = blackboard->open_for_writing<RobotinoSensorInterface>("Robotino");
<   imu_if_    = blackboard->open_for_writing<IMUInterface>("IMU Robotino");
< 
<   //Create suscribers
<   pos_sub_ = gazebonode->Subscribe(config->get_string("/gazsim/topics/gps"), &RobotinoSimThread::on_pos_msg, this);
<   gyro_sub_ = gazebonode->Subscribe(config->get_string("/gazsim/topics/gyro"), &RobotinoSimThread::on_gyro_msg, this);
<   infrared_puck_sensor_sub_ = gazebonode->Subscribe(config->get_string("/gazsim/topics/infrared-puck-sensor"), &RobotinoSimThread::on_infrared_puck_sensor_msg, this);
<   if(have_gripper_sensors_)
<   {
<     gripper_laser_left_sensor_sub_ = gazebonode->Subscribe(config->get_string("/gazsim/topics/gripper-laser-left"), &RobotinoSimThread::on_gripper_laser_left_sensor_msg, this);
<     gripper_laser_right_sensor_sub_ = gazebonode->Subscribe(config->get_string("/gazsim/topics/gripper-laser-right"), &RobotinoSimThread::on_gripper_laser_right_sensor_msg, this);
<   }
< 
<   //Create publishers
<   motor_move_pub_ = gazebonode->Advertise<msgs::Vector3d>(config->get_string("/gazsim/topics/motor-move"));
<   string_pub_ = gazebonode->Advertise<msgs::Header>(config->get_string("/gazsim/topics/message"));
< 
<   //enable motor by default
<   switch_if_->set_enabled(true);
<   switch_if_->write();
< 
<   imu_if_->set_linear_acceleration(0, -1.);
<   //imu_if_->set_angular_velocity_covariance(8, deg2rad(0.1));
<   // set as not available as we do not currently provide angular velocities.
<   imu_if_->set_angular_velocity_covariance(0, -1.);
<   imu_if_->write();
< 
<   //init motor variables
<   x_ = 0.0;
<   y_ = 0.0;
<   ori_ = 0.0;
<   vx_ = 0.0;
<   vy_ = 0.0;
<   vomega_ = 0.0;
<   des_vx_ = 0.0;
<   des_vy_ = 0.0;
<   des_vomega_ = 0.0;
<   x_offset_ = 0.0;
<   y_offset_ = 0.0;
<   ori_offset_ = 0.0;
<   path_length_ = 0.0;
< 
<   gyro_buffer_index_new_ = 0;
<   gyro_buffer_index_delayed_ = 0;
<   gyro_timestamp_buffer_ = new fawkes::Time[gyro_buffer_size_];
<   gyro_angle_buffer_ = new float[gyro_buffer_size_];
< 
<   new_data_ = false;
< 
<   if(string_pub_->HasConnections())
<   {
<     msgs::Header helloMessage;
<     helloMessage.set_str_id("gazsim-robotino plugin connected");
<     string_pub_->Publish(helloMessage);
---
> 	//get a connection to gazebo (copied from gazeboscene)
> 	logger->log_debug(name(), "Creating Gazebo publishers");
148c64,157
<   }
---
> 	//read config values
> 	cfg_frame_odom_            = config->get_string("/frames/odom");
> 	cfg_frame_base_            = config->get_string("/frames/base");
> 	cfg_frame_imu_             = config->get_string("/gazsim/robotino/imu/frame");
> 	slippery_wheels_enabled_   = config->get_bool("gazsim/robotino/motor/slippery-wheels-enabled");
> 	slippery_wheels_threshold_ = config->get_float("gazsim/robotino/motor/slippery-wheels-threshold");
> 	moving_speed_factor_       = config->get_float("gazsim/robotino/motor/moving-speed-factor");
> 	rotation_speed_factor_     = config->get_float("gazsim/robotino/motor/rotation-speed-factor");
> 	gripper_laser_threshold_   = config->get_float("/gazsim/robotino/gripper-laser-threshold");
> 	gripper_laser_value_far_   = config->get_float("/gazsim/robotino/gripper-laser-value-far");
> 	gripper_laser_value_near_  = config->get_float("/gazsim/robotino/gripper-laser-value-near");
> 	have_gripper_sensors_      = config->exists("/hardware/robotino/sensors/right_ir_id")
> 	                        && config->exists("/hardware/robotino/sensors/left_ir_id");
> 	if (have_gripper_sensors_) {
> 		gripper_laser_right_pos_ = config->get_int("/hardware/robotino/sensors/right_ir_id");
> 		gripper_laser_left_pos_  = config->get_int("/hardware/robotino/sensors/left_ir_id");
> 	}
> 	gyro_buffer_size_      = config->get_int("/gazsim/robotino/gyro-buffer-size");
> 	gyro_delay_            = config->get_float("/gazsim/robotino/gyro-delay");
> 	infrared_sensor_index_ = config->get_int("/gazsim/robotino/infrared-sensor-index");
> 
> 	tf_enable_publisher(cfg_frame_base_.c_str());
> 
> 	//Open interfaces
> 	motor_if_  = blackboard->open_for_writing<MotorInterface>("Robotino");
> 	switch_if_ = blackboard->open_for_writing<fawkes::SwitchInterface>("Robotino Motor");
> 	sens_if_   = blackboard->open_for_writing<RobotinoSensorInterface>("Robotino");
> 	imu_if_    = blackboard->open_for_writing<IMUInterface>("IMU Robotino");
> 
> 	//Create suscribers
> 	pos_sub_  = gazebonode->Subscribe(config->get_string("/gazsim/topics/gps"),
>                                    &RobotinoSimThread::on_pos_msg,
>                                    this);
> 	gyro_sub_ = gazebonode->Subscribe(config->get_string("/gazsim/topics/gyro"),
> 	                                  &RobotinoSimThread::on_gyro_msg,
> 	                                  this);
> 	infrared_puck_sensor_sub_ =
> 	  gazebonode->Subscribe(config->get_string("/gazsim/topics/infrared-puck-sensor"),
> 	                        &RobotinoSimThread::on_infrared_puck_sensor_msg,
> 	                        this);
> 	if (have_gripper_sensors_) {
> 		gripper_laser_left_sensor_sub_ =
> 		  gazebonode->Subscribe(config->get_string("/gazsim/topics/gripper-laser-left"),
> 		                        &RobotinoSimThread::on_gripper_laser_left_sensor_msg,
> 		                        this);
> 		gripper_laser_right_sensor_sub_ =
> 		  gazebonode->Subscribe(config->get_string("/gazsim/topics/gripper-laser-right"),
> 		                        &RobotinoSimThread::on_gripper_laser_right_sensor_msg,
> 		                        this);
> 	}
> 
> 	//Create publishers
> 	motor_move_pub_ =
> 	  gazebonode->Advertise<msgs::Vector3d>(config->get_string("/gazsim/topics/motor-move"));
> 	string_pub_ = gazebonode->Advertise<msgs::Header>(config->get_string("/gazsim/topics/message"));
> 
> 	//enable motor by default
> 	switch_if_->set_enabled(true);
> 	switch_if_->write();
> 
> 	imu_if_->set_frame(cfg_frame_imu_.c_str());
> 	imu_if_->set_linear_acceleration(0, -1.);
> 	//imu_if_->set_angular_velocity_covariance(8, deg2rad(0.1));
> 	// set as not available as we do not currently provide angular velocities.
> 	imu_if_->set_angular_velocity_covariance(0, -1.);
> 	imu_if_->write();
> 
> 	//init motor variables
> 	x_           = 0.0;
> 	y_           = 0.0;
> 	ori_         = 0.0;
> 	vx_          = 0.0;
> 	vy_          = 0.0;
> 	vomega_      = 0.0;
> 	des_vx_      = 0.0;
> 	des_vy_      = 0.0;
> 	des_vomega_  = 0.0;
> 	x_offset_    = 0.0;
> 	y_offset_    = 0.0;
> 	ori_offset_  = 0.0;
> 	path_length_ = 0.0;
> 
> 	gyro_buffer_index_new_     = 0;
> 	gyro_buffer_index_delayed_ = 0;
> 	gyro_timestamp_buffer_     = new fawkes::Time[gyro_buffer_size_];
> 	gyro_angle_buffer_         = new float[gyro_buffer_size_];
> 
> 	new_data_ = false;
> 
> 	if (string_pub_->HasConnections()) {
> 		msgs::Header helloMessage;
> 		helloMessage.set_str_id("gazsim-robotino plugin connected");
> 		string_pub_->Publish(helloMessage);
> 	}
154,158c163,167
<   //close interfaces
<   blackboard->close(imu_if_);
<   blackboard->close(sens_if_);
<   blackboard->close(motor_if_);
<   blackboard->close(switch_if_);
---
> 	//close interfaces
> 	blackboard->close(imu_if_);
> 	blackboard->close(sens_if_);
> 	blackboard->close(motor_if_);
> 	blackboard->close(switch_if_);
160,161c169,170
<   delete [] gyro_timestamp_buffer_;
<   delete [] gyro_angle_buffer_;
---
> 	delete[] gyro_timestamp_buffer_;
> 	delete[] gyro_angle_buffer_;
167,168c176,227
<   //work off all messages passed to the motor_interfaces
<   process_motor_messages();
---
> 	//work off all messages passed to the motor_interfaces
> 	process_motor_messages();
> 
> 	//update interfaces
> 	if (new_data_) {
> 		motor_if_->set_odometry_position_x(x_);
> 		motor_if_->set_odometry_position_y(y_);
> 		motor_if_->set_odometry_orientation(ori_);
> 		motor_if_->set_odometry_path_length(path_length_);
> 		motor_if_->write();
> 
> 		if (gyro_available_) {
> 			//update gyro (with delay)
> 			fawkes::Time now(clock);
> 			while ((now - gyro_timestamp_buffer_[(gyro_buffer_index_delayed_ + 1) % gyro_buffer_size_])
> 			           .in_sec()
> 			         >= gyro_delay_
> 			       && gyro_buffer_index_delayed_ < gyro_buffer_index_new_) {
> 				gyro_buffer_index_delayed_++;
> 			}
> 
> 			tf::Quaternion q =
> 			  tf::create_quaternion_from_yaw(gyro_angle_buffer_[gyro_buffer_index_delayed_]);
> 			imu_if_->set_orientation(0, q.x());
> 			imu_if_->set_orientation(1, q.y());
> 			imu_if_->set_orientation(2, q.z());
> 			imu_if_->set_orientation(3, q.w());
> 			for (uint i = 0; i < 9u; i += 4) {
> 				imu_if_->set_orientation_covariance(i, 1e-3);
> 				imu_if_->set_angular_velocity_covariance(i, 1e-3);
> 				imu_if_->set_linear_acceleration_covariance(i, 1e-3);
> 			}
> 		} else {
> 			imu_if_->set_angular_velocity(0, -1.);
> 			imu_if_->set_orientation(0, -1.);
> 			imu_if_->set_orientation(1, 0.);
> 			imu_if_->set_orientation(2, 0.);
> 			imu_if_->set_orientation(3, 0.);
> 		}
> 		imu_if_->write();
> 
> 		sens_if_->set_distance(infrared_sensor_index_, infrared_puck_sensor_dist_);
> 
> 		if (have_gripper_sensors_) {
> 			sens_if_->set_analog_in(gripper_laser_left_pos_, analog_in_left_);
> 			sens_if_->set_analog_in(gripper_laser_right_pos_, analog_in_right_);
> 		}
> 		sens_if_->write();
> 
> 		new_data_ = false;
> 	}
> }
170,292c229,236
<   //update interfaces
<   if(new_data_)
<   {
<     motor_if_->set_odometry_position_x(x_);
<     motor_if_->set_odometry_position_y(y_);
<     motor_if_->set_odometry_orientation(ori_);
<     motor_if_->set_odometry_path_length(path_length_);
<     motor_if_->write();
< 
<     if(gyro_available_)
<     {
<       //update gyro (with delay)
<       fawkes::Time now(clock);
<       while ((now - gyro_timestamp_buffer_[(gyro_buffer_index_delayed_ + 1) % gyro_buffer_size_]).in_sec() >= gyro_delay_
< 	     && gyro_buffer_index_delayed_ < gyro_buffer_index_new_)
<       {
< 	gyro_buffer_index_delayed_++;
<       }
< 	  
<       tf::Quaternion q =
< 	tf::create_quaternion_from_yaw(gyro_angle_buffer_[gyro_buffer_index_delayed_]);
<       imu_if_->set_orientation(0, q.x());
<       imu_if_->set_orientation(1, q.y());
<       imu_if_->set_orientation(2, q.z());
<       imu_if_->set_orientation(3, q.w());
<     } else {
<       imu_if_->set_angular_velocity(0, -1.);
<       imu_if_->set_orientation(0, -1.);
<       imu_if_->set_orientation(1,  0.);
<       imu_if_->set_orientation(2,  0.);
<       imu_if_->set_orientation(3,  0.);
<     }
<     imu_if_->write();
< 
<     sens_if_->set_distance(infrared_sensor_index_, infrared_puck_sensor_dist_);
<     
<     if(have_gripper_sensors_)
<     {
<       sens_if_->set_analog_in(gripper_laser_left_pos_, analog_in_left_);
<       sens_if_->set_analog_in(gripper_laser_right_pos_, analog_in_right_);
<     }    
<     sens_if_->write();
< 
<     new_data_ = false;
<   }
< }
< 
< void RobotinoSimThread::send_transroot(double vx, double vy, double omega)
< {
<   msgs::Vector3d motorMove;
<   motorMove.set_x(vx_);
<   motorMove.set_y(vy_);
<   motorMove.set_z(vomega_);
<   motor_move_pub_->Publish(motorMove);
< }
< 
< void RobotinoSimThread::process_motor_messages()
< {
<   //check messages of the switch interface
<   while (!switch_if_->msgq_empty()) {
<     if (SwitchInterface::DisableSwitchMessage *msg =
< 	switch_if_->msgq_first_safe(msg)) {
<       switch_if_->set_enabled(false);
<       //pause movement
<       send_transroot(0, 0, 0);
<     } else if (SwitchInterface::EnableSwitchMessage *msg =
< 	       switch_if_->msgq_first_safe(msg)) {
<       switch_if_->set_enabled(true);
<       //unpause movement
<       send_transroot(vx_, vy_, vomega_);
<     }
<     switch_if_->msgq_pop();
<     switch_if_->write();
<   }
< 
<   //do not do anything if the motor is disabled
<   if(!switch_if_->is_enabled())
<   {
<     return;
<   }
< 
<   //check messages of the motor interface
<   while(motor_move_pub_->HasConnections() && !motor_if_->msgq_empty())
<   {
<     if (MotorInterface::TransRotMessage *msg =
< 	motor_if_->msgq_first_safe(msg))
<     {
<       //send command only if changed
<       if(vel_changed(msg->vx(), vx_, 0.01) || vel_changed(msg->vy(), vy_, 0.01) || vel_changed(msg->omega(), vomega_, 0.01))
<       {
< 	vx_ = msg->vx();
< 	vy_ = msg->vy();
< 	vomega_ = msg->omega();
< 	des_vx_ = vx_;
< 	des_vy_ = vy_;
< 	des_vomega_ = vomega_;
< 	
< 	//send message to gazebo (apply movement_factor to compensate friction)
< 	send_transroot(vx_ * moving_speed_factor_, vy_ * moving_speed_factor_, vomega_ * rotation_speed_factor_);
< 
< 	//update interface
< 	motor_if_->set_vx(vx_);
< 	motor_if_->set_vy(vy_);
< 	motor_if_->set_omega(vomega_);
< 	motor_if_->set_des_vx(des_vx_);
< 	motor_if_->set_des_vy(des_vy_);
< 	motor_if_->set_des_omega(des_vomega_);
< 	//update interface
< 	motor_if_->write();
<       }    
<     }
<     else if (motor_if_->msgq_first_is<MotorInterface::ResetOdometryMessage>())
<       {
<         x_offset_ += x_;
< 	y_offset_ += y_;
<         ori_offset_ += ori_;
< 	x_ = 0.0;
< 	y_ = 0.0;
< 	ori_ = 0.0;
< 	last_vel_set_time_ = clock->now();
<       }
<     motor_if_->msgq_pop();
<   }
---
> void
> RobotinoSimThread::send_transroot(double vx, double vy, double omega)
> {
> 	msgs::Vector3d motorMove;
> 	motorMove.set_x(vx_);
> 	motorMove.set_y(vy_);
> 	motorMove.set_z(vomega_);
> 	motor_move_pub_->Publish(motorMove);
295c239,240
< bool RobotinoSimThread::vel_changed(float before, float after, float relativeThreashold)
---
> void
> RobotinoSimThread::process_motor_messages()
297c242,300
<   return(before == 0.0 || after == 0.0 || fabs((before-after)/before) > relativeThreashold);
---
> 	//check messages of the switch interface
> 	while (!switch_if_->msgq_empty()) {
> 		if (SwitchInterface::DisableSwitchMessage *msg = switch_if_->msgq_first_safe(msg)) {
> 			switch_if_->set_enabled(false);
> 			//pause movement
> 			send_transroot(0, 0, 0);
> 		} else if (SwitchInterface::EnableSwitchMessage *msg = switch_if_->msgq_first_safe(msg)) {
> 			switch_if_->set_enabled(true);
> 			//unpause movement
> 			send_transroot(vx_, vy_, vomega_);
> 		}
> 		switch_if_->msgq_pop();
> 		switch_if_->write();
> 	}
> 
> 	//do not do anything if the motor is disabled
> 	if (!switch_if_->is_enabled()) {
> 		return;
> 	}
> 
> 	//check messages of the motor interface
> 	while (motor_move_pub_->HasConnections() && !motor_if_->msgq_empty()) {
> 		if (MotorInterface::TransRotMessage *msg = motor_if_->msgq_first_safe(msg)) {
> 			//send command only if changed
> 			if (vel_changed(msg->vx(), vx_, 0.01) || vel_changed(msg->vy(), vy_, 0.01)
> 			    || vel_changed(msg->omega(), vomega_, 0.01)) {
> 				vx_         = msg->vx();
> 				vy_         = msg->vy();
> 				vomega_     = msg->omega();
> 				des_vx_     = vx_;
> 				des_vy_     = vy_;
> 				des_vomega_ = vomega_;
> 
> 				//send message to gazebo (apply movement_factor to compensate friction)
> 				send_transroot(vx_ * moving_speed_factor_,
> 				               vy_ * moving_speed_factor_,
> 				               vomega_ * rotation_speed_factor_);
> 
> 				//update interface
> 				motor_if_->set_vx(vx_);
> 				motor_if_->set_vy(vy_);
> 				motor_if_->set_omega(vomega_);
> 				motor_if_->set_des_vx(des_vx_);
> 				motor_if_->set_des_vy(des_vy_);
> 				motor_if_->set_des_omega(des_vomega_);
> 				//update interface
> 				motor_if_->write();
> 			}
> 		} else if (motor_if_->msgq_first_is<MotorInterface::ResetOdometryMessage>()) {
> 			x_offset_ += x_;
> 			y_offset_ += y_;
> 			ori_offset_ += ori_;
> 			x_                 = 0.0;
> 			y_                 = 0.0;
> 			ori_               = 0.0;
> 			last_vel_set_time_ = clock->now();
> 		}
> 		motor_if_->msgq_pop();
> 	}
299a303,307
> bool
> RobotinoSimThread::vel_changed(float before, float after, float relativeThreashold)
> {
> 	return (before == 0.0 || after == 0.0 || fabs((before - after) / before) > relativeThreashold);
> }
302c310,311
< void RobotinoSimThread::on_pos_msg(ConstPosePtr &msg)
---
> void
> RobotinoSimThread::on_pos_msg(ConstPosePtr &msg)
304c313
<   //logger_->log_debug(name_, "Got Position MSG from gazebo with ori: %f", msg->z());
---
> 	//logger_->log_debug(name_, "Got Position MSG from gazebo with ori: %f", msg->z());
306c315,403
<   MutexLocker lock(loop_mutex);
---
> 	MutexLocker lock(loop_mutex);
> 
> 	//read out values + substract offset
> 	float new_x = msg->position().x() - x_offset_;
> 	float new_y = msg->position().y() - y_offset_;
> 	//calculate ori from quaternion
> 	float new_ori = tf::get_yaw(tf::Quaternion(msg->orientation().x(),
> 	                                           msg->orientation().y(),
> 	                                           msg->orientation().z(),
> 	                                           msg->orientation().w()));
> 	new_ori -= ori_offset_;
> 
> 	//estimate path-length
> 	float length_driven = sqrt((new_x - x_) * (new_x - x_) + (new_y - y_) * (new_y - y_));
> 
> 	if (slippery_wheels_enabled_) {
> 		//simulate slipping wheels when driving against an obstacle
> 		fawkes::Time new_time = clock->now();
> 		double       duration = new_time.in_sec() - last_pos_time_.in_sec();
> 		//calculate duration since the velocity was last set to filter slipping while accelerating
> 		double velocity_set_duration = new_time.in_sec() - last_vel_set_time_.in_sec();
> 
> 		last_pos_time_ = new_time;
> 
> 		double total_speed = sqrt(vx_ * vx_ + vy_ * vy_);
> 		if (length_driven < total_speed * duration * slippery_wheels_threshold_
> 		    && velocity_set_duration > duration) {
> 			double speed_abs_x = vx_ * cos(ori_) - vy_ * sin(ori_);
> 			double speed_abs_y = vx_ * sin(ori_) + vy_ * cos(ori_);
> 			double slipped_x   = speed_abs_x * duration * slippery_wheels_threshold_;
> 			double slipped_y   = speed_abs_y * duration * slippery_wheels_threshold_;
> 			//logger_->log_debug(name_, "Wheels are slipping (%f, %f)", slipped_x, slipped_y);
> 			new_x = x_ + slipped_x;
> 			new_y = y_ + slipped_y;
> 			//update the offset (otherwise the slippery error would be corrected in the next iteration)
> 			x_offset_ -= slipped_x;
> 			y_offset_ -= slipped_y;
> 
> 			length_driven = sqrt((new_x - x_) * (new_x - x_) + (new_y - y_) * (new_y - y_));
> 		}
> 	}
> 
> 	//update stored values
> 	x_   = new_x;
> 	y_   = new_y;
> 	ori_ = new_ori;
> 	path_length_ += length_driven;
> 	new_data_ = true;
> 
> 	//publish transform (otherwise the transform can not convert /base_link to /odom)
> 	fawkes::Time  now(clock);
> 	tf::Transform t(tf::Quaternion(tf::Vector3(0, 0, 1), ori_), tf::Vector3(x_, y_, 0.0));
> 
> 	tf_publisher->send_transform(t, now, cfg_frame_odom_, cfg_frame_base_);
> }
> void
> RobotinoSimThread::on_gyro_msg(ConstVector3dPtr &msg)
> {
> 	MutexLocker lock(loop_mutex);
> 	gyro_buffer_index_new_                         = (gyro_buffer_index_new_ + 1) % gyro_buffer_size_;
> 	gyro_angle_buffer_[gyro_buffer_index_new_]     = msg->z();
> 	gyro_timestamp_buffer_[gyro_buffer_index_new_] = clock->now();
> 	gyro_available_                                = true;
> 	new_data_                                      = true;
> }
> void
> RobotinoSimThread::on_infrared_puck_sensor_msg(ConstLaserScanStampedPtr &msg)
> {
> 	MutexLocker lock(loop_mutex);
> 	//make sure that the config values for fetch_puck are set right
> 	infrared_puck_sensor_dist_ = msg->scan().ranges(0);
> 	new_data_                  = true;
> }
> void
> RobotinoSimThread::on_gripper_laser_right_sensor_msg(ConstFloatPtr &msg)
> {
> 	MutexLocker lock(loop_mutex);
> 
> 	if (msg->value() < gripper_laser_threshold_) {
> 		analog_in_right_ = gripper_laser_value_near_;
> 	} else {
> 		analog_in_right_ = gripper_laser_value_far_;
> 	}
> 	new_data_ = true;
> }
> void
> RobotinoSimThread::on_gripper_laser_left_sensor_msg(ConstFloatPtr &msg)
> {
> 	MutexLocker lock(loop_mutex);
308,404c405,410
<   //read out values + substract offset
<   float new_x = msg->position().x() - x_offset_;
<   float new_y = msg->position().y() - y_offset_;
<   //calculate ori from quaternion
<   float new_ori = tf::get_yaw(tf::Quaternion(msg->orientation().x(), msg->orientation().y()
< 					  , msg->orientation().z(), msg->orientation().w()));
<   new_ori -= ori_offset_;
<   
<   //estimate path-length
<   float length_driven = sqrt((new_x-x_) * (new_x-x_) + (new_y-y_) * (new_y-y_));
<   
<   if(slippery_wheels_enabled_)
<   {
<     //simulate slipping wheels when driving against an obstacle
<     fawkes::Time new_time = clock->now();
<     double duration = new_time.in_sec() - last_pos_time_.in_sec();
<     //calculate duration since the velocity was last set to filter slipping while accelerating
<     double velocity_set_duration = new_time.in_sec() - last_vel_set_time_.in_sec();
< 
<     last_pos_time_ = new_time;
<     
< 
<     double total_speed = sqrt(vx_ * vx_ + vy_ * vy_);
<     if(length_driven < total_speed * duration * slippery_wheels_threshold_ && velocity_set_duration > duration)
<     {
<       double speed_abs_x = vx_ * cos(ori_) - vy_ * sin(ori_);
<       double speed_abs_y = vx_ * sin(ori_) + vy_ * cos(ori_);
<       double slipped_x = speed_abs_x * duration * slippery_wheels_threshold_;
<       double slipped_y = speed_abs_y * duration * slippery_wheels_threshold_;
<       //logger_->log_debug(name_, "Wheels are slipping (%f, %f)", slipped_x, slipped_y);
<       new_x = x_ + slipped_x;
<       new_y = y_ + slipped_y;
<       //update the offset (otherwise the slippery error would be corrected in the next iteration)
<       x_offset_ -= slipped_x;
<       y_offset_ -= slipped_y;	      
< 
<       length_driven = sqrt((new_x-x_) * (new_x-x_) + (new_y-y_) * (new_y-y_));
<     }
<   }
< 
<   //update stored values
<   x_ = new_x;
<   y_ = new_y;
<   ori_ = new_ori;
<   path_length_ += length_driven;
<   new_data_ = true;
< 
<   //publish transform (otherwise the transform can not convert /base_link to /odom)
<   fawkes::Time now(clock);
<   tf::Transform t(tf::Quaternion(tf::Vector3(0,0,1),ori_),
< 		  tf::Vector3(x_, y_, 0.0));
< 
<   tf_publisher->send_transform(t, now, cfg_frame_odom_, cfg_frame_base_);
< }
< void RobotinoSimThread::on_gyro_msg(ConstVector3dPtr &msg)
< {
<   MutexLocker lock(loop_mutex);
<   gyro_buffer_index_new_ = (gyro_buffer_index_new_ + 1) % gyro_buffer_size_;
<   gyro_angle_buffer_[gyro_buffer_index_new_] = msg->z();
<   gyro_timestamp_buffer_[gyro_buffer_index_new_] = clock->now();
<   gyro_available_ = true;
<   new_data_ = true;
< }
< void RobotinoSimThread::on_infrared_puck_sensor_msg(ConstLaserScanStampedPtr &msg)
< {
<   MutexLocker lock(loop_mutex);
<   //make sure that the config values for fetch_puck are set right
<   infrared_puck_sensor_dist_ = msg->scan().ranges(0);
<   new_data_ = true;
< }
< void RobotinoSimThread::on_gripper_laser_right_sensor_msg(ConstFloatPtr &msg)
< {
<   MutexLocker lock(loop_mutex);
< 
<   if(msg->value() < gripper_laser_threshold_)
<   {
<     analog_in_right_ = gripper_laser_value_near_;
<   }
<   else
<   {
<     analog_in_right_ = gripper_laser_value_far_;
<   }
<   new_data_ = true;
< }
< void RobotinoSimThread::on_gripper_laser_left_sensor_msg(ConstFloatPtr &msg)
< {
<   MutexLocker lock(loop_mutex);
< 
<   if(msg->value() < gripper_laser_threshold_)
<   {
<     analog_in_left_ = gripper_laser_value_near_;
<   }
<   else
<   {
<     analog_in_left_ = gripper_laser_value_far_;
<   }
<   new_data_ = true;
---
> 	if (msg->value() < gripper_laser_threshold_) {
> 		analog_in_left_ = gripper_laser_value_near_;
> 	} else {
> 		analog_in_left_ = gripper_laser_value_far_;
> 	}
> 	new_data_ = true;
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-robotino/gazsim_robotino_thread.h fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-robotino/gazsim_robotino_thread.h
23,24c23,24
< #ifndef __PLUGINS_GAZSIM_ROBOTINO_THREAD_H_
< #define __PLUGINS_GAZSIM_ROBOTINO_THREAD_H_
---
> #ifndef _PLUGINS_GAZSIM_ROBOTINO_THREAD_H_
> #define _PLUGINS_GAZSIM_ROBOTINO_THREAD_H_
26c26
< #include <list>
---
> #include "../msgs/Float.pb.h"
28c28,29
< #include <core/threading/thread.h>
---
> #include <aspect/blackboard.h>
> #include <aspect/blocked_timing.h>
32,34d32
< #include <aspect/blackboard.h>
< #include <aspect/blocked_timing.h>
< #include <plugins/gazebo/aspect/gazebo.h>
36c34,37
< #include "../msgs/Float.pb.h"
---
> #include <core/threading/thread.h>
> #include <plugins/gazebo/aspect/gazebo.h>
> 
> #include <list>
39d39
< #include <gazebo/transport/TransportTypes.hh>
40a41
> #include <gazebo/transport/TransportTypes.hh>
43d43
< 
47,62c47,61
<   class BatteryInterface;
<   class IMUInterface;
<   class MotorInterface;
<   class RobotinoSensorInterface;
<   class SwitchInterface;
< }
< 
< class RobotinoSimThread
< : public fawkes::Thread,
<   public fawkes::ClockAspect,
<   public fawkes::LoggingAspect,
<   public fawkes::ConfigurableAspect,
<   public fawkes::BlackBoardAspect,
<   public fawkes::BlockedTimingAspect,
<   public fawkes::TransformAspect,
<   public fawkes::GazeboAspect
---
> class BatteryInterface;
> class IMUInterface;
> class MotorInterface;
> class RobotinoSensorInterface;
> class SwitchInterface;
> } // namespace fawkes
> 
> class RobotinoSimThread : public fawkes::Thread,
>                           public fawkes::ClockAspect,
>                           public fawkes::LoggingAspect,
>                           public fawkes::ConfigurableAspect,
>                           public fawkes::BlackBoardAspect,
>                           public fawkes::BlockedTimingAspect,
>                           public fawkes::TransformAspect,
>                           public fawkes::GazeboAspect
64,65c63,64
<  public:
<   RobotinoSimThread();
---
> public:
> 	RobotinoSimThread();
67,151c66,152
<   virtual void init();
<   virtual void loop();
<   virtual void finalize();
<  private:
<   //Publisher to send messages to gazebo
<   gazebo::transport::PublisherPtr string_pub_;
<   gazebo::transport::PublisherPtr motor_move_pub_;
< 
<   //Suscribers to recieve messages from gazebo
<   gazebo::transport::SubscriberPtr gyro_sub_;
<   gazebo::transport::SubscriberPtr infrared_puck_sensor_sub_;
<   gazebo::transport::SubscriberPtr gripper_laser_left_sensor_sub_;
<   gazebo::transport::SubscriberPtr gripper_laser_right_sensor_sub_;
<   gazebo::transport::SubscriberPtr pos_sub_;
< 
<   //Handler functions for incoming messages
<   void on_gyro_msg(ConstVector3dPtr &msg);
<   void on_infrared_puck_sensor_msg(ConstLaserScanStampedPtr &msg);
<   void on_gripper_laser_left_sensor_msg(ConstFloatPtr &msg);
<   void on_gripper_laser_right_sensor_msg(ConstFloatPtr &msg);
<   void on_pos_msg(ConstPosePtr &msg);
< 
<   //provided interfaces
<   fawkes::RobotinoSensorInterface *sens_if_;
<   fawkes::MotorInterface          *motor_if_;
<   fawkes::SwitchInterface         *switch_if_;
<   fawkes::IMUInterface            *imu_if_;
< 
<   //config values
<   std::string cfg_frame_odom_;
<   std::string cfg_frame_base_;
<   double gripper_laser_threshold_;
<   double gripper_laser_value_far_;
<   double gripper_laser_value_near_;
<   bool slippery_wheels_enabled_;
<   double slippery_wheels_threshold_;
<   double moving_speed_factor_;
<   double rotation_speed_factor_;
<   bool have_gripper_sensors_;
<   int gripper_laser_left_pos_;
<   int gripper_laser_right_pos_;
<   int infrared_sensor_index_;
< 
<   //Helper variables for motor:
< 
<   //current motorMovements
<   float vx_;
<   float vy_;
<   float vomega_;
<   float des_vx_;
<   float des_vy_;
<   float des_vomega_;
<   //last received odom position
<   float x_;
<   float y_;
<   float ori_;
<   float path_length_;
<  
<   //RobotinoSensorInterface values (stored here to write the interfaces only in the loop)
<   bool gyro_available_;
<   int gyro_buffer_size_;
<   int gyro_buffer_index_new_;
<   int gyro_buffer_index_delayed_;
<   fawkes::Time *gyro_timestamp_buffer_;
<   float *gyro_angle_buffer_;
<   float gyro_delay_;
<   float infrared_puck_sensor_dist_;
<   float analog_in_left_;
<   float analog_in_right_;
< 
<   //are there new values to write in the interfaces?
<   bool new_data_;
< 
<   fawkes::Time last_pos_time_;
<   fawkes::Time last_vel_set_time_;
< 
<   //Odometry offset
<   float x_offset_;
<   float y_offset_;
<   float ori_offset_;
< 
<   //Helper functions:
<   void process_motor_messages();
<   void send_transroot(double vx, double vy, double omega);
<   bool vel_changed(float before, float after, float relativeThreashold);  
---
> 	virtual void init();
> 	virtual void loop();
> 	virtual void finalize();
> 
> private:
> 	//Publisher to send messages to gazebo
> 	gazebo::transport::PublisherPtr string_pub_;
> 	gazebo::transport::PublisherPtr motor_move_pub_;
> 
> 	//Suscribers to recieve messages from gazebo
> 	gazebo::transport::SubscriberPtr gyro_sub_;
> 	gazebo::transport::SubscriberPtr infrared_puck_sensor_sub_;
> 	gazebo::transport::SubscriberPtr gripper_laser_left_sensor_sub_;
> 	gazebo::transport::SubscriberPtr gripper_laser_right_sensor_sub_;
> 	gazebo::transport::SubscriberPtr pos_sub_;
> 
> 	//Handler functions for incoming messages
> 	void on_gyro_msg(ConstVector3dPtr &msg);
> 	void on_infrared_puck_sensor_msg(ConstLaserScanStampedPtr &msg);
> 	void on_gripper_laser_left_sensor_msg(ConstFloatPtr &msg);
> 	void on_gripper_laser_right_sensor_msg(ConstFloatPtr &msg);
> 	void on_pos_msg(ConstPosePtr &msg);
> 
> 	//provided interfaces
> 	fawkes::RobotinoSensorInterface *sens_if_;
> 	fawkes::MotorInterface *         motor_if_;
> 	fawkes::SwitchInterface *        switch_if_;
> 	fawkes::IMUInterface *           imu_if_;
> 
> 	//config values
> 	std::string cfg_frame_odom_;
> 	std::string cfg_frame_base_;
> 	std::string cfg_frame_imu_;
> 	double      gripper_laser_threshold_;
> 	double      gripper_laser_value_far_;
> 	double      gripper_laser_value_near_;
> 	bool        slippery_wheels_enabled_;
> 	double      slippery_wheels_threshold_;
> 	double      moving_speed_factor_;
> 	double      rotation_speed_factor_;
> 	bool        have_gripper_sensors_;
> 	int         gripper_laser_left_pos_;
> 	int         gripper_laser_right_pos_;
> 	int         infrared_sensor_index_;
> 
> 	//Helper variables for motor:
> 
> 	//current motorMovements
> 	float vx_;
> 	float vy_;
> 	float vomega_;
> 	float des_vx_;
> 	float des_vy_;
> 	float des_vomega_;
> 	//last received odom position
> 	float x_;
> 	float y_;
> 	float ori_;
> 	float path_length_;
> 
> 	//RobotinoSensorInterface values (stored here to write the interfaces only in the loop)
> 	bool          gyro_available_;
> 	int           gyro_buffer_size_;
> 	int           gyro_buffer_index_new_;
> 	int           gyro_buffer_index_delayed_;
> 	fawkes::Time *gyro_timestamp_buffer_;
> 	float *       gyro_angle_buffer_;
> 	float         gyro_delay_;
> 	float         infrared_puck_sensor_dist_;
> 	float         analog_in_left_;
> 	float         analog_in_right_;
> 
> 	//are there new values to write in the interfaces?
> 	bool new_data_;
> 
> 	fawkes::Time last_pos_time_;
> 	fawkes::Time last_vel_set_time_;
> 
> 	//Odometry offset
> 	float x_offset_;
> 	float y_offset_;
> 	float ori_offset_;
> 
> 	//Helper functions:
> 	void process_motor_messages();
> 	void send_transroot(double vx, double vy, double omega);
> 	bool vel_changed(float before, float after, float relativeThreashold);
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-robotino/Makefile fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-robotino/Makefile
27,31c27,31
< LIBS_gazsim_robotino = fawkescore fawkesutils fawkesaspects fvutils \
< 		fawkestf fawkesinterface fawkesblackboard \
< 		BatteryInterface RobotinoSensorInterface MotorInterface \
< 		Position3DInterface IMUInterface SwitchInterface \
< 		fawkesgazeboaspect gazsim_msgs \
---
> LIBS_gazsim_robotino = m fawkescore fawkesutils fawkesaspects fvutils \
>                        fawkestf fawkesinterface fawkesblackboard \
>                        BatteryInterface RobotinoSensorInterface MotorInterface \
>                        Position3DInterface IMUInterface SwitchInterface \
>                        fawkesgazeboaspect gazsim_msgs \
35a36
> PLUGINS_all = $(PLUGINDIR)/gazsim-robotino.so
43c44
<   PLUGINS_all = $(PLUGINDIR)/gazsim-robotino.so
---
>   PLUGINS_build = $(PLUGINS_all)
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-timesource/gazsim_timesource_plugin.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-timesource/gazsim_timesource_plugin.cpp
23a24
> 
33,34c34,35
<  public:
<   /** Constructor.
---
> public:
> 	/** Constructor.
37,40c38,41
<   GazsimTimesourcePlugin(Configuration *config) : Plugin(config)
<   {
<     thread_list.push_back(new GazsimTimesourceThread());
<   }
---
> 	explicit GazsimTimesourcePlugin(Configuration *config) : Plugin(config)
> 	{
> 		thread_list.push_back(new GazsimTimesourceThread());
> 	}
42d42
< 
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-timesource/gazsim_timesource_source.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-timesource/gazsim_timesource_source.cpp
31c31
< GazsimTimesource::GazsimTimesource(Clock* clock)
---
> GazsimTimesource::GazsimTimesource(Clock *clock)
33c33
<   clock_ = clock;
---
> 	clock_ = clock;
35,38c35,38
<   last_sim_time_ = get_system_time();
<   last_real_time_factor_ = 1.0;
<   clock_->get_systime(&last_sys_recv_time_);
<   //registration will be done by plugin
---
> 	last_sim_time_         = get_system_time();
> 	last_real_time_factor_ = 1.0;
> 	clock_->get_systime(&last_sys_recv_time_);
> 	//registration will be done by plugin
43d42
< 
52c51
< GazsimTimesource::get_time(timeval* tv) const
---
> GazsimTimesource::get_time(timeval *tv) const
54,62c53,62
<   //I do not use the Time - operator here because this would recursively call get_time
<   timeval now = get_system_time();
<   timeval interval = subtract(now, last_sys_recv_time_);  
< 
<   //doing this: timeval estimated_sim_interval = interval * last_real_time_factor_;
<   timeval estimated_sim_interval;
<   estimated_sim_interval.tv_usec = last_real_time_factor_ * (interval.tv_sec * 1000000  + interval.tv_usec);
<   estimated_sim_interval.tv_sec = estimated_sim_interval.tv_usec / 1000000;
<   estimated_sim_interval.tv_usec -= estimated_sim_interval.tv_sec * 1000000;
---
> 	//I do not use the Time - operator here because this would recursively call get_time
> 	timeval now      = get_system_time();
> 	timeval interval = subtract(now, last_sys_recv_time_);
> 
> 	//doing this: timeval estimated_sim_interval = interval * last_real_time_factor_;
> 	timeval estimated_sim_interval;
> 	estimated_sim_interval.tv_usec =
> 	  last_real_time_factor_ * (interval.tv_sec * 1000000 + interval.tv_usec);
> 	estimated_sim_interval.tv_sec = estimated_sim_interval.tv_usec / 1000000;
> 	estimated_sim_interval.tv_usec -= estimated_sim_interval.tv_sec * 1000000;
64c64
<   timeval estimated_sim_now = add(last_sim_time_, estimated_sim_interval);
---
> 	timeval estimated_sim_now = add(last_sim_time_, estimated_sim_interval);
66,67c66,67
<   //return
<   *tv =  estimated_sim_now;
---
> 	//return
> 	*tv = estimated_sim_now;
71c71
< GazsimTimesource::conv_to_realtime(const timeval* tv) const
---
> GazsimTimesource::conv_to_realtime(const timeval *tv) const
73c73
<   timeval interval = subtract(*tv, last_sim_time_);
---
> 	timeval interval = subtract(*tv, last_sim_time_);
75,79c75,80
<   //doing this: timeval est_real_interval = interval / last_real_time_factor_;
<   timeval est_real_interval;
<   est_real_interval.tv_usec = (interval.tv_sec * 1000000  + interval.tv_usec) / last_real_time_factor_;
<   est_real_interval.tv_sec = est_real_interval.tv_usec / 1000000;
<   est_real_interval.tv_usec -= est_real_interval.tv_sec * 1000000;
---
> 	//doing this: timeval est_real_interval = interval / last_real_time_factor_;
> 	timeval est_real_interval;
> 	est_real_interval.tv_usec =
> 	  (interval.tv_sec * 1000000 + interval.tv_usec) / last_real_time_factor_;
> 	est_real_interval.tv_sec = est_real_interval.tv_usec / 1000000;
> 	est_real_interval.tv_usec -= est_real_interval.tv_sec * 1000000;
81,82c82,83
<   timeval result = add(last_sys_recv_time_, est_real_interval);
<   return result;
---
> 	timeval result = add(last_sys_recv_time_, est_real_interval);
> 	return result;
86c87
< GazsimTimesource::conv_native_to_exttime(const timeval* tv) const
---
> GazsimTimesource::conv_native_to_exttime(const timeval *tv) const
88,95c89,96
<   timeval t_offset    = subtract(*tv, last_native_sim_time_);
<   double  offset      = t_offset.tv_sec + t_offset.tv_usec / 1000000.f;
<   long    offset_sec  = ::ceil(offset);
<   long    offset_usec = ::round(offset - offset_sec) * 1000000;
< 
<   timeval rv;
<   rv.tv_sec  = last_sim_time_.tv_sec  + offset_sec;
<   rv.tv_usec = last_sim_time_.tv_usec + offset_usec;
---
> 	timeval t_offset    = subtract(*tv, last_native_sim_time_);
> 	double  offset      = t_offset.tv_sec + t_offset.tv_usec / 1000000.f;
> 	long    offset_sec  = ::ceil(offset);
> 	long    offset_usec = ::round(offset - offset_sec) * 1000000;
> 
> 	timeval rv;
> 	rv.tv_sec  = last_sim_time_.tv_sec + offset_sec;
> 	rv.tv_usec = last_sim_time_.tv_usec + offset_usec;
97c98
<   return rv;
---
> 	return rv;
106,111c107,112
<   //we do not want to correct time back
<   get_time(&last_sim_time_);
<   last_real_time_factor_ = msg->real_time_factor();
<   clock_->get_systime(&last_sys_recv_time_);
<   last_native_sim_time_.tv_sec  = msg->sim_time_sec();
<   last_native_sim_time_.tv_usec = msg->sim_time_nsec() / 1000;
---
> 	//we do not want to correct time back
> 	get_time(&last_sim_time_);
> 	last_real_time_factor_ = msg->real_time_factor();
> 	clock_->get_systime(&last_sys_recv_time_);
> 	last_native_sim_time_.tv_sec  = msg->sim_time_sec();
> 	last_native_sim_time_.tv_usec = msg->sim_time_nsec() / 1000;
117,119c118,120
<   timeval now_timeval;
<   gettimeofday(&now_timeval,NULL);
<   return now_timeval;
---
> 	timeval now_timeval;
> 	gettimeofday(&now_timeval, NULL);
> 	return now_timeval;
125,133c126,133
<   timeval res;
<   res.tv_sec = a.tv_sec + b.tv_sec;
<   res.tv_usec = a.tv_usec + b.tv_usec;
<   if(res.tv_usec > 1000000)
<   {
<     res.tv_usec -= 1000000;
<     res.tv_sec++;
<   }
<   return res;
---
> 	timeval res;
> 	res.tv_sec  = a.tv_sec + b.tv_sec;
> 	res.tv_usec = a.tv_usec + b.tv_usec;
> 	if (res.tv_usec > 1000000) {
> 		res.tv_usec -= 1000000;
> 		res.tv_sec++;
> 	}
> 	return res;
139,150c139,147
<   timeval res;
<   res.tv_sec = a.tv_sec - b.tv_sec;
<   if(a.tv_usec >= b.tv_usec)
<   {
<     res.tv_usec = a.tv_usec - b.tv_usec;
<   }
<   else
<   {
<     res.tv_usec = 1000000 + a.tv_usec - b.tv_usec;
<     res.tv_sec--;
<   }
<   return res;
---
> 	timeval res;
> 	res.tv_sec = a.tv_sec - b.tv_sec;
> 	if (a.tv_usec >= b.tv_usec) {
> 		res.tv_usec = a.tv_usec - b.tv_usec;
> 	} else {
> 		res.tv_usec = 1000000 + a.tv_usec - b.tv_usec;
> 		res.tv_sec--;
> 	}
> 	return res;
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-timesource/gazsim_timesource_source.h fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-timesource/gazsim_timesource_source.h
23,26d22
< #include <boost/asio.hpp>
< #include <utils/time/timesource.h>
< #include <utils/time/clock.h>
< 
29,30c25,26
< #ifndef __GAZEBO_TIMESOURCE_H_
< #define __GAZEBO_TIMESOURCE_H_
---
> #include <utils/time/clock.h>
> #include <utils/time/timesource.h>
31a28,31
> #include <boost/asio.hpp>
> 
> #ifndef _GAZEBO_TIMESOURCE_H_
> #	define _GAZEBO_TIMESOURCE_H_
35,37c35,36
< namespace fawkes
< {
<   /** @class GazsimTimesource
---
> namespace fawkes {
> /** @class GazsimTimesource
41,70c40,68
<   class GazsimTimesource : public TimeSource
<   {
<   public:
<     //Constructor
<     GazsimTimesource(Clock* clock);
<     ///Destructor
<    ~GazsimTimesource();
< 
<     virtual void get_time(timeval* tv) const;
<     virtual timeval conv_to_realtime(const timeval* tv) const;
<     virtual timeval conv_native_to_exttime(const timeval* tv) const;
< 
<     /// store data from gazebo time message
<     void on_time_sync_msg(ConstSimTimePtr &msg);
< 
<   private:
<     timeval get_system_time() const;
<     timeval add(timeval a, timeval b) const;
<     timeval subtract(timeval a, timeval b) const;
< 
<   private:
<     Clock* clock_;
< 
<     //from last msg all in sec
<     timeval last_sim_time_;
<     timeval last_sys_recv_time_;
<     double  last_real_time_factor_;
<     timeval last_native_sim_time_;
< 
<   };
---
> class GazsimTimesource : public TimeSource
> {
> public:
> 	//Constructor
> 	GazsimTimesource(Clock *clock);
> 	///Destructor
> 	~GazsimTimesource();
> 
> 	virtual void    get_time(timeval *tv) const;
> 	virtual timeval conv_to_realtime(const timeval *tv) const;
> 	virtual timeval conv_native_to_exttime(const timeval *tv) const;
> 
> 	/// store data from gazebo time message
> 	void on_time_sync_msg(ConstSimTimePtr &msg);
> 
> private:
> 	timeval get_system_time() const;
> 	timeval add(timeval a, timeval b) const;
> 	timeval subtract(timeval a, timeval b) const;
> 
> private:
> 	Clock *clock_;
> 
> 	//from last msg all in sec
> 	timeval last_sim_time_;
> 	timeval last_sys_recv_time_;
> 	double  last_real_time_factor_;
> 	timeval last_native_sim_time_;
> };
72c70
< }
---
> } // namespace fawkes
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-timesource/gazsim_timesource_thread.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-timesource/gazsim_timesource_thread.cpp
33,34c33,34
<   : Thread("GazsimTimesourceThread", Thread::OPMODE_WAITFORWAKEUP),
<     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE)
---
> : Thread("GazsimTimesourceThread", Thread::OPMODE_WAITFORWAKEUP),
>   BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE)
42,43c42,43
< 
< void GazsimTimesourceThread::init()
---
> void
> GazsimTimesourceThread::init()
45c45
<   logger->log_info(name(), "GazsimTimesource initializing");
---
> 	logger->log_info(name(), "GazsimTimesource initializing");
47,48c47,50
<   //Create Subscriber
<   time_sync_sub_ = gazebo_world_node->Subscribe(config->get_string("/gazsim/topics/time"), &GazsimTimesourceThread::on_time_sync_msg, this);
---
> 	//Create Subscriber
> 	time_sync_sub_ = gazebo_world_node->Subscribe(config->get_string("/gazsim/topics/time"),
> 	                                              &GazsimTimesourceThread::on_time_sync_msg,
> 	                                              this);
50,51c52,53
<   //Create Time Source
<   time_source_ = new GazsimTimesource(clock);
---
> 	//Create Time Source
> 	time_source_ = new GazsimTimesource(clock);
53,54c55,56
<   //register timesource and make it default
<   clock->register_ext_timesource(time_source_, true);
---
> 	//register timesource and make it default
> 	clock->register_ext_timesource(time_source_, true);
57,58c59,60
< 
< void GazsimTimesourceThread::finalize()
---
> void
> GazsimTimesourceThread::finalize()
60,62c62,64
<   //remove time source
<   clock->remove_ext_timesource(time_source_);
<   delete time_source_;
---
> 	//remove time source
> 	clock->remove_ext_timesource(time_source_);
> 	delete time_source_;
65,66c67,68
< 
< void GazsimTimesourceThread::loop()
---
> void
> GazsimTimesourceThread::loop()
68c70
<   //nothing interesting
---
> 	//nothing interesting
71c73,74
< void GazsimTimesourceThread::on_time_sync_msg(ConstSimTimePtr &msg)
---
> void
> GazsimTimesourceThread::on_time_sync_msg(ConstSimTimePtr &msg)
73,76c76,79
<   // logger->log_info(name(), "Got Simulation Time");
<   
<   //provide time source with newest message
<   time_source_->on_time_sync_msg(msg);
---
> 	// logger->log_info(name(), "Got Simulation Time");
> 
> 	//provide time source with newest message
> 	time_source_->on_time_sync_msg(msg);
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-timesource/gazsim_timesource_thread.h fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-timesource/gazsim_timesource_thread.h
23,24c23,24
< #ifndef __PLUGINS_GAZSIM__TIMESOURCE_THREAD_H_
< #define __PLUGINS_GAZSIM__TIMESOURCE_THREAD_H_
---
> #ifndef _PLUGINS_GAZSIMTIMESOURCE_THREAD_H__
> #define _PLUGINS_GAZSIMTIMESOURCE_THREAD_H__
26c26,29
< #include <core/threading/thread.h>
---
> #include "../msgs/SimTime.pb.h"
> #include "gazsim_timesource_source.h"
> 
> #include <aspect/blocked_timing.h>
28d30
< #include <aspect/logging.h>
30,31c32,33
< #include <aspect/blocked_timing.h>
< #include <boost/asio.hpp>
---
> #include <aspect/logging.h>
> #include <core/threading/thread.h>
33d34
< #include <gazebo/physics/physics.hh>
36,37c37,38
< #include "gazsim_timesource_source.h"
< #include "../msgs/SimTime.pb.h"
---
> #include <boost/asio.hpp>
> #include <gazebo/physics/physics.hh>
41,47c42,47
< class GazsimTimesourceThread
< : public fawkes::Thread,
<   public fawkes::ClockAspect,
<   public fawkes::BlockedTimingAspect,
<   public fawkes::ConfigurableAspect,
<   public fawkes::LoggingAspect,
<   public fawkes::GazeboAspect
---
> class GazsimTimesourceThread : public fawkes::Thread,
>                                public fawkes::ClockAspect,
>                                public fawkes::BlockedTimingAspect,
>                                public fawkes::ConfigurableAspect,
>                                public fawkes::LoggingAspect,
>                                public fawkes::GazeboAspect
49,62c49,59
<  public:
<   GazsimTimesourceThread();
<   ~GazsimTimesourceThread();
< 
<   virtual void init();
<   virtual void loop();
<   virtual void finalize();
< 
<  private:
<   //Timesource to give the fawkes clock
<   fawkes::GazsimTimesource* time_source_;
< 
<   //subscriber to get time msgs from
<   gazebo::transport::SubscriberPtr time_sync_sub_;
---
> public:
> 	GazsimTimesourceThread();
> 	~GazsimTimesourceThread();
> 
> 	virtual void init();
> 	virtual void loop();
> 	virtual void finalize();
> 
> private:
> 	//Timesource to give the fawkes clock
> 	fawkes::GazsimTimesource *time_source_;
64,65c61,62
<   //handler
<   void on_time_sync_msg(ConstSimTimePtr &msg);
---
> 	//subscriber to get time msgs from
> 	gazebo::transport::SubscriberPtr time_sync_sub_;
66a64,65
> 	//handler
> 	void on_time_sync_msg(ConstSimTimePtr &msg);
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-timesource/Makefile fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-timesource/Makefile
25d24
< 
31c30,31
< OBJS_all = $(OBJS_gazsim_timesource)
---
> OBJS_all    = $(OBJS_gazsim_timesource)
> PLUGINS_all = $(PLUGINDIR)/gazsim-timesource.so
38,39c38,39
< 	     $(call boost-libs-cflags,$(REQ_BOOST_LIBS)) \
< 	     $(CFLAGS_GAZEBO)
---
>              $(call boost-libs-cflags,$(REQ_BOOST_LIBS)) \
>              $(CFLAGS_GAZEBO)
41,42c41,42
< 	     $(call boost-libs-ldflags,$(REQ_BOOST_LIBS)) \
< 	     $(LDFLAGS_GAZEBO)
---
>              $(call boost-libs-ldflags,$(REQ_BOOST_LIBS)) \
>              $(LDFLAGS_GAZEBO)
44c44
<   PLUGINS_all = $(PLUGINDIR)/gazsim-timesource.so
---
>   PLUGINS_build = $(PLUGINS_all)
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-vis-localization/gazsim_vis_localization_plugin.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-vis-localization/gazsim_vis_localization_plugin.cpp
21,22d20
< #include <core/plugin.h>
< 
24a23,24
> #include <core/plugin.h>
> 
34,36c34,35
<  public:
< 
<   /** Constructor.
---
> public:
> 	/** Constructor.
39,43c38,41
<   GazsimVisLocalizationPlugin(Configuration *config)
<     : Plugin(config)
<   {
<     thread_list.push_back(new VisLocalizationThread());
<   }
---
> 	explicit GazsimVisLocalizationPlugin(Configuration *config) : Plugin(config)
> 	{
> 		thread_list.push_back(new VisLocalizationThread());
> 	}
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-vis-localization/gazsim_vis_localization_thread.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-vis-localization/gazsim_vis_localization_thread.cpp
22a23,24
> #include <aspect/logging.h>
> #include <interfaces/Position3DInterface.h>
24,25d25
< #include <stdio.h>
< #include <cmath>
28,30c28
< #include <interfaces/Position3DInterface.h>
< 
< #include <gazebo/transport/Node.hh>
---
> #include <cmath>
31a30
> #include <gazebo/transport/Node.hh>
33c32
< #include <aspect/logging.h>
---
> #include <stdio.h>
45,46c44,45
<   : Thread("VisLocalizationThread", Thread::OPMODE_WAITFORWAKEUP),
<     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE)
---
> : Thread("VisLocalizationThread", Thread::OPMODE_WAITFORWAKEUP),
>   BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE)
50c49,50
< void VisLocalizationThread::init()
---
> void
> VisLocalizationThread::init()
52c52
<   logger->log_debug(name(), "Initializing Visualization of the Localization");
---
> 	logger->log_debug(name(), "Initializing Visualization of the Localization");
54,63c54,63
<   //read config values
<   update_rate_ = config->get_float("/gazsim/visualization/localization/update-rate");
<   robot_name_ = config->get_string("/gazsim/robot-name");
<   label_script_name_ = config->get_string("/gazsim/visualization/label-script-name");
<   arrow_script_name_ = config->get_string("/gazsim/visualization/label-arrow-name");
<   location_scripts_ = config->get_string("/gazsim/visualization/location-scripts");
<   location_textures_ = config->get_string("/gazsim/visualization/location-textures");
<   parent_name_ = config->get_string("/gazsim/visualization/localization/parent-name");
<   label_size_ = config->get_float("/gazsim/visualization/localization/label-size");
<   label_height_ = config->get_float("/gazsim/visualization/localization/label-height");
---
> 	//read config values
> 	update_rate_       = config->get_float("/gazsim/visualization/localization/update-rate");
> 	robot_name_        = config->get_string("/gazsim/robot-name");
> 	label_script_name_ = config->get_string("/gazsim/visualization/label-script-name");
> 	arrow_script_name_ = config->get_string("/gazsim/visualization/label-arrow-name");
> 	location_scripts_  = config->get_string("/gazsim/visualization/location-scripts");
> 	location_textures_ = config->get_string("/gazsim/visualization/location-textures");
> 	parent_name_       = config->get_string("/gazsim/visualization/localization/parent-name");
> 	label_size_        = config->get_float("/gazsim/visualization/localization/label-size");
> 	label_height_      = config->get_float("/gazsim/visualization/localization/label-height");
65c65
<   last_update_time_ = clock->now().in_sec();
---
> 	last_update_time_ = clock->now().in_sec();
67,68c67,68
<   //open interface
<   pose_if_ = blackboard->open_for_reading<Position3DInterface>("Pose");
---
> 	//open interface
> 	pose_if_ = blackboard->open_for_reading<Position3DInterface>("Pose");
70,71c70,71
<   //create publisher
<   visual_publisher_ = gazebo_world_node->Advertise<gazebo::msgs::Visual>("~/visual", 5);
---
> 	//create publisher
> 	visual_publisher_ = gazebo_world_node->Advertise<gazebo::msgs::Visual>("~/visual", 5);
74c74,75
< void VisLocalizationThread::finalize()
---
> void
> VisLocalizationThread::finalize()
76c77
<   blackboard->close(pose_if_);
---
> 	blackboard->close(pose_if_);
79c80,81
< void VisLocalizationThread::loop()
---
> void
> VisLocalizationThread::loop()
81,105c83,105
<   //visualize the estimated position of the robot every few seconds
<   fawkes::Time new_time = clock->now();
<   double time_elapsed = new_time.in_sec() - last_update_time_.in_sec();
<   if(time_elapsed > 1 / update_rate_)
<   {
<     last_update_time_ = new_time;
< 
<     //read pose
<     pose_if_->read();
<     double x = pose_if_->translation(0);
<     double y = pose_if_->translation(1);
<     //calculate ori from quaternion in interface
<     double* quat = pose_if_->rotation();
<     double ori = tf::get_yaw(tf::Quaternion(quat[0], quat[1], quat[2], quat[3]));
<     if(std::isnan(ori))
<     {
<       ori = 0.0;
<     }
< 
<     //create label with number
<     msgs::Visual msg_number;
<     msg_number.set_name((robot_name_ + "-localization-label").c_str());
<     msg_number.set_parent_name(parent_name_.c_str());
<     msgs::Geometry *geomMsg = msg_number.mutable_geometry();
<     geomMsg->set_type(msgs::Geometry::PLANE);
---
> 	//visualize the estimated position of the robot every few seconds
> 	fawkes::Time new_time     = clock->now();
> 	double       time_elapsed = new_time.in_sec() - last_update_time_.in_sec();
> 	if (time_elapsed > 1 / update_rate_) {
> 		last_update_time_ = new_time;
> 
> 		//read pose
> 		pose_if_->read();
> 		double x = pose_if_->translation(0);
> 		double y = pose_if_->translation(1);
> 		//calculate ori from quaternion in interface
> 		double *quat = pose_if_->rotation();
> 		double  ori  = tf::get_yaw(tf::Quaternion(quat[0], quat[1], quat[2], quat[3]));
> 		if (std::isnan(ori)) {
> 			ori = 0.0;
> 		}
> 
> 		//create label with number
> 		msgs::Visual msg_number;
> 		msg_number.set_name((robot_name_ + "-localization-label").c_str());
> 		msg_number.set_parent_name(parent_name_.c_str());
> 		msgs::Geometry *geomMsg = msg_number.mutable_geometry();
> 		geomMsg->set_type(msgs::Geometry::PLANE);
107,108c107,109
<     msgs::Set(geomMsg->mutable_plane()->mutable_normal(), ignition::math::Vector3d(0.0, 0.0, 1.0));
<     msgs::Set(geomMsg->mutable_plane()->mutable_size(), ignition::math::Vector2d(label_size_, label_size_));
---
> 		msgs::Set(geomMsg->mutable_plane()->mutable_normal(), ignition::math::Vector3d(0.0, 0.0, 1.0));
> 		msgs::Set(geomMsg->mutable_plane()->mutable_size(),
> 		          ignition::math::Vector2d(label_size_, label_size_));
110,112c111,113
<     msgs::Set(geomMsg->mutable_plane()->mutable_normal(), math::Vector3(0.0, 0.0, 1.0));
<     msgs::Set(geomMsg->mutable_plane()->mutable_size(), math::Vector2d(label_size_, label_size_));
<     msg_number.set_transparency(0.2);  
---
> 		msgs::Set(geomMsg->mutable_plane()->mutable_normal(), math::Vector3(0.0, 0.0, 1.0));
> 		msgs::Set(geomMsg->mutable_plane()->mutable_size(), math::Vector2d(label_size_, label_size_));
> 		msg_number.set_transparency(0.2);
114c115
<     msg_number.set_cast_shadows(false);
---
> 		msg_number.set_cast_shadows(false);
116c117
<     msgs::Set(msg_number.mutable_pose(), ignition::math::Pose3d(x, y, label_height_, 0, 0, 0));
---
> 		msgs::Set(msg_number.mutable_pose(), ignition::math::Pose3d(x, y, label_height_, 0, 0, 0));
118c119
<     msgs::Set(msg_number.mutable_pose(), math::Pose(x, y, label_height_, 0, 0, 0));
---
> 		msgs::Set(msg_number.mutable_pose(), math::Pose(x, y, label_height_, 0, 0, 0));
120,123c121,124
<     msgs::Material::Script* script = msg_number.mutable_material()->mutable_script();
<     script->add_uri(location_scripts_.c_str());
<     script->add_uri(location_textures_.c_str());
<     script->set_name(label_script_name_.c_str());
---
> 		msgs::Material::Script *script = msg_number.mutable_material()->mutable_script();
> 		script->add_uri(location_scripts_.c_str());
> 		script->add_uri(location_textures_.c_str());
> 		script->set_name(label_script_name_.c_str());
125c126
<     visual_publisher_->Publish(msg_number);
---
> 		visual_publisher_->Publish(msg_number);
127c128
<     //create label with direction arrow
---
> 		//create label with direction arrow
129,151c130,156
<     msgs::Visual msg_arrow;
<     msg_arrow.set_name((robot_name_ + "-localization-arrow").c_str());
<     msg_arrow.set_parent_name(parent_name_.c_str());
<     msgs::Geometry *geomArrowMsg = msg_arrow.mutable_geometry();
<     geomArrowMsg->set_type(msgs::Geometry::PLANE);
< #if GAZEBO_MAJOR_VERSION > 5
<     msgs::Set(geomArrowMsg->mutable_plane()->mutable_normal(), ignition::math::Vector3d(0.0, 0.0, 1.0));
<     msgs::Set(geomArrowMsg->mutable_plane()->mutable_size(), ignition::math::Vector2d(0.17, 0.17));
< #else
<     msgs::Set(geomArrowMsg->mutable_plane()->mutable_normal(), math::Vector3(0.0, 0.0, 1.0));
<     msgs::Set(geomArrowMsg->mutable_plane()->mutable_size(), math::Vector2d(0.17, 0.17));
<     msg_arrow.set_transparency(0.4);  
< #endif
<     msg_arrow.set_cast_shadows(false);
< #if GAZEBO_MAJOR_VERSION > 5
<     msgs::Set(msg_arrow.mutable_pose(), ignition::math::Pose3d(x, y, label_height_ + 0.01, 0, 0, ori - /*turn image right*/ M_PI / 2));
< #else
<     msgs::Set(msg_arrow.mutable_pose(), math::Pose(x, y, label_height_ + 0.01, 0, 0, ori - /*turn image right*/ M_PI / 2));
< #endif
<     msgs::Material::Script* arrow_script = msg_arrow.mutable_material()->mutable_script();
<     arrow_script->add_uri(location_scripts_.c_str());
<     arrow_script->add_uri(location_textures_.c_str());
<     arrow_script->set_name(arrow_script_name_.c_str());
---
> 		msgs::Visual msg_arrow;
> 		msg_arrow.set_name((robot_name_ + "-localization-arrow").c_str());
> 		msg_arrow.set_parent_name(parent_name_.c_str());
> 		msgs::Geometry *geomArrowMsg = msg_arrow.mutable_geometry();
> 		geomArrowMsg->set_type(msgs::Geometry::PLANE);
> #	if GAZEBO_MAJOR_VERSION > 5
> 		msgs::Set(geomArrowMsg->mutable_plane()->mutable_normal(),
> 		          ignition::math::Vector3d(0.0, 0.0, 1.0));
> 		msgs::Set(geomArrowMsg->mutable_plane()->mutable_size(), ignition::math::Vector2d(0.17, 0.17));
> #	else
> 		msgs::Set(geomArrowMsg->mutable_plane()->mutable_normal(), math::Vector3(0.0, 0.0, 1.0));
> 		msgs::Set(geomArrowMsg->mutable_plane()->mutable_size(), math::Vector2d(0.17, 0.17));
> 		msg_arrow.set_transparency(0.4);
> #	endif
> 		msg_arrow.set_cast_shadows(false);
> #	if GAZEBO_MAJOR_VERSION > 5
> 		msgs::Set(msg_arrow.mutable_pose(),
> 		          ignition::math::Pose3d(
> 		            x, y, label_height_ + 0.01, 0, 0, ori - /*turn image right*/ M_PI / 2));
> #	else
> 		msgs::Set(msg_arrow.mutable_pose(),
> 		          math::Pose(x, y, label_height_ + 0.01, 0, 0, ori - /*turn image right*/ M_PI / 2));
> #	endif
> 		msgs::Material::Script *arrow_script = msg_arrow.mutable_material()->mutable_script();
> 		arrow_script->add_uri(location_scripts_.c_str());
> 		arrow_script->add_uri(location_textures_.c_str());
> 		arrow_script->set_name(arrow_script_name_.c_str());
153c158
<     visual_publisher_->Publish(msg_arrow);
---
> 		visual_publisher_->Publish(msg_arrow);
155c160
<   }
---
> 	}
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-vis-localization/gazsim_vis_localization_thread.h fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-vis-localization/gazsim_vis_localization_thread.h
21,22c21,22
< #ifndef __PLUGINS_GAZSIM_VIS_LOCALIZATION_THREAD_H_
< #define __PLUGINS_GAZSIM_VIS_LOCALIZATION_THREAD_H_
---
> #ifndef _PLUGINS_GAZSIM_VIS_LOCALIZATION_THREAD_H_
> #define _PLUGINS_GAZSIM_VIS_LOCALIZATION_THREAD_H_
24c24,25
< #include <core/threading/thread.h>
---
> #include <aspect/blackboard.h>
> #include <aspect/blocked_timing.h>
28,29c29
< #include <aspect/blackboard.h>
< #include <aspect/blocked_timing.h>
---
> #include <core/threading/thread.h>
32a33
> 
36d36
< #include <gazebo/transport/TransportTypes.hh>
37a38
> #include <gazebo/transport/TransportTypes.hh>
40d40
< 
42c42
<   class Position3DInterface;
---
> class Position3DInterface;
45,52c45,51
< class VisLocalizationThread
< : public fawkes::Thread,
<   public fawkes::ClockAspect,
<   public fawkes::LoggingAspect,
<   public fawkes::ConfigurableAspect,
<   public fawkes::BlackBoardAspect,
<   public fawkes::BlockedTimingAspect,
<   public fawkes::GazeboAspect
---
> class VisLocalizationThread : public fawkes::Thread,
>                               public fawkes::ClockAspect,
>                               public fawkes::LoggingAspect,
>                               public fawkes::ConfigurableAspect,
>                               public fawkes::BlackBoardAspect,
>                               public fawkes::BlockedTimingAspect,
>                               public fawkes::GazeboAspect
54,55c53,54
<  public:
<   VisLocalizationThread();
---
> public:
> 	VisLocalizationThread();
57,74c56,74
<   virtual void init();
<   virtual void loop();
<   virtual void finalize();
< 
<  private:
<   //read pose interface
<   fawkes::Position3DInterface *pose_if_;
< 
<   //Publisher for visual msgs
<   gazebo::transport::PublisherPtr visual_publisher_;
< 
<   double update_rate_;
<   fawkes::Time last_update_time_;
< 
<   //config values
<   std::string robot_name_, label_script_name_, location_scripts_, location_textures_, parent_name_, arrow_script_name_;
<   float label_size_;
<   float label_height_;
---
> 	virtual void init();
> 	virtual void loop();
> 	virtual void finalize();
> 
> private:
> 	//read pose interface
> 	fawkes::Position3DInterface *pose_if_;
> 
> 	//Publisher for visual msgs
> 	gazebo::transport::PublisherPtr visual_publisher_;
> 
> 	double       update_rate_;
> 	fawkes::Time last_update_time_;
> 
> 	//config values
> 	std::string robot_name_, label_script_name_, location_scripts_, location_textures_, parent_name_,
> 	  arrow_script_name_;
> 	float label_size_;
> 	float label_height_;
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-vis-localization/Makefile fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-vis-localization/Makefile
25c25
< LIBS_gazsim_vis_localization = fawkescore fawkesutils fawkesaspects fvutils \
---
> LIBS_gazsim_vis_localization = m fawkescore fawkesutils fawkesaspects fvutils \
31a32
> PLUGINS_all = $(PLUGINDIR)/gazsim-vis-localization.so
37c38,39
<   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) -lm $(call boost-libs-ldflags,system) -lboost_system
---
>   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) \
>              $(call boost-libs-ldflags,system)
39c41
<   PLUGINS_all = $(PLUGINDIR)/gazsim-vis-localization.so
---
>   PLUGINS_build = $(PLUGINS_all)
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-webcam/gazsim_webcam.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-webcam/gazsim_webcam.cpp
22a23
> #include <fvutils/color/conversions.h>
24,25d24
< #include <stdio.h>
< #include <math.h>
28d26
< #include <gazebo/transport/Node.hh>
29a28
> #include <gazebo/transport/Node.hh>
31,32c30,31
< 
< #include <fvutils/color/conversions.h>
---
> #include <math.h>
> #include <stdio.h>
41d39
< 
48,50c46,48
< GazsimWebcam::GazsimWebcam(std::string shm_id,
< 			     gazebo::transport::NodePtr gazebo_world_node,
< 			     Configuration* config)
---
> GazsimWebcam::GazsimWebcam(std::string                shm_id,
>                            gazebo::transport::NodePtr gazebo_world_node,
>                            Configuration *            config)
52,84c50,78
<   shm_buffer_ = NULL;
<   //read config values
<   std::string robot_name = config->get_string("/gazsim/robot-name");
<   shm_id_ = robot_name + "/" + shm_id;
<   topic_name_ = ("~/" 
< 		 + robot_name
<                  + config->get_string((std::string("/gazsim/webcam/topic-suffixes/")
< 		                        + shm_id).c_str()));
<   width_ = config->get_float((std::string("/gazsim/webcam/widths/") + shm_id).c_str());
<   height_ = config->get_float((std::string("/gazsim/webcam/heights/") + shm_id).c_str());
<   frame_ = config->get_string((std::string("/gazsim/webcam/frames/") + shm_id).c_str());
< 
<   format_from_ = firevision::RGB;
<   format_to_ = firevision::YUV422_PLANAR;
< 
<   //subscribing to gazebo publisher
<   //the messages are published by the sensor itself and not by a robot plugin
<   //therefore we have to use the world node
<   webcam_sub_ = gazebo_world_node->Subscribe(topic_name_, &GazsimWebcam::on_webcam_data_msg, this);
< 
<   //initialize shared memory image buffer
<   shm_buffer_ = new firevision::SharedMemoryImageBuffer( shm_id_.c_str(),
< 							 format_to_,
< 							 width_,
< 							 height_
< 							 );
<   if (!shm_buffer_->is_valid()) {
<     throw fawkes::Exception("Shared memory segment not valid");
<   }
<   shm_buffer_->set_frame_id(frame_.c_str());
<   buffer_ = shm_buffer_->buffer();
<   //enable locking
<   shm_buffer_->add_semaphore();
---
> 	shm_buffer_ = NULL;
> 	//read config values
> 	std::string robot_name = config->get_string("/gazsim/robot-name");
> 	shm_id_                = robot_name + "/" + shm_id;
> 	topic_name_ =
> 	  ("~/" + robot_name
> 	   + config->get_string((std::string("/gazsim/webcam/topic-suffixes/") + shm_id).c_str()));
> 	width_  = config->get_float((std::string("/gazsim/webcam/widths/") + shm_id).c_str());
> 	height_ = config->get_float((std::string("/gazsim/webcam/heights/") + shm_id).c_str());
> 	frame_  = config->get_string((std::string("/gazsim/webcam/frames/") + shm_id).c_str());
> 
> 	format_from_ = firevision::RGB;
> 	format_to_   = firevision::YUV422_PLANAR;
> 
> 	//subscribing to gazebo publisher
> 	//the messages are published by the sensor itself and not by a robot plugin
> 	//therefore we have to use the world node
> 	webcam_sub_ = gazebo_world_node->Subscribe(topic_name_, &GazsimWebcam::on_webcam_data_msg, this);
> 
> 	//initialize shared memory image buffer
> 	shm_buffer_ =
> 	  new firevision::SharedMemoryImageBuffer(shm_id_.c_str(), format_to_, width_, height_);
> 	if (!shm_buffer_->is_valid()) {
> 		throw fawkes::Exception("Shared memory segment not valid");
> 	}
> 	shm_buffer_->set_frame_id(frame_.c_str());
> 	buffer_ = shm_buffer_->buffer();
> 	//enable locking
> 	shm_buffer_->add_semaphore();
89c83
<   delete this->shm_buffer_;
---
> 	delete this->shm_buffer_;
92,100c86,98
< void GazsimWebcam::on_webcam_data_msg(ConstImageStampedPtr &msg)
< {  
<   //convert image data and write it in the shared memory buffer
<   //lock the shm so noone can read a half written image
<   shm_buffer_->lock_for_write();
<   convert(format_from_,  format_to_,
<  	  (const unsigned char*) msg->image().data().data(),   buffer_,
< 	  width_, height_);
<   shm_buffer_->unlock();
---
> void
> GazsimWebcam::on_webcam_data_msg(ConstImageStampedPtr &msg)
> {
> 	//convert image data and write it in the shared memory buffer
> 	//lock the shm so noone can read a half written image
> 	shm_buffer_->lock_for_write();
> 	convert(format_from_,
> 	        format_to_,
> 	        (const unsigned char *)msg->image().data().data(),
> 	        buffer_,
> 	        width_,
> 	        height_);
> 	shm_buffer_->unlock();
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-webcam/gazsim_webcam.h fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-webcam/gazsim_webcam.h
21,22c21,22
< #ifndef __PLUGINS_GAZSIM_WEBCAM_H_
< #define __PLUGINS_GAZSIM_WEBCAM_H_
---
> #ifndef _PLUGINS_GAZSIM_WEBCAM_H_
> #define _PLUGINS_GAZSIM_WEBCAM_H_
24c24,25
< #include <core/threading/thread.h>
---
> #include <aspect/blackboard.h>
> #include <aspect/blocked_timing.h>
28,31d28
< #include <aspect/blackboard.h>
< #include <aspect/blocked_timing.h>
< #include <plugins/gazebo/aspect/gazebo.h>
< #include <string.h>
33c30
< 
---
> #include <core/threading/thread.h>
34a32,33
> #include <plugins/gazebo/aspect/gazebo.h>
> 
35a35
> #include <string.h>
38d37
< #include <gazebo/transport/TransportTypes.hh>
39a39
> #include <gazebo/transport/TransportTypes.hh>
42d41
< 
45,71c44,70
<  public:
<   GazsimWebcam(std::string shm_id,
< 	       gazebo::transport::NodePtr gazebo_world_node,
< 	       fawkes::Configuration* config);
<   ~GazsimWebcam();
< 
<  private:  
<   //Subscriber to receive webcam data from gazebo
<   gazebo::transport::SubscriberPtr webcam_sub_;
< 
<   //handler function for incoming webcam data messages
<   void on_webcam_data_msg(ConstImageStampedPtr &msg);
< 
<   //shared memory buffer
<   firevision::SharedMemoryImageBuffer *shm_buffer_;
<   //reference to the buffer of shm_buffer_
<   unsigned char *buffer_;
< 
<   //config values
<   //topic name of the gazebo message publisher
<   std::string topic_name_;
<   double width_, height_;
<   //id of the shared memory object
<   std::string shm_id_;
<   std::string frame_;
<   firevision::colorspace_t format_from_;
<   firevision::colorspace_t format_to_;
---
> public:
> 	GazsimWebcam(std::string                shm_id,
> 	             gazebo::transport::NodePtr gazebo_world_node,
> 	             fawkes::Configuration *    config);
> 	~GazsimWebcam();
> 
> private:
> 	//Subscriber to receive webcam data from gazebo
> 	gazebo::transport::SubscriberPtr webcam_sub_;
> 
> 	//handler function for incoming webcam data messages
> 	void on_webcam_data_msg(ConstImageStampedPtr &msg);
> 
> 	//shared memory buffer
> 	firevision::SharedMemoryImageBuffer *shm_buffer_;
> 	//reference to the buffer of shm_buffer_
> 	unsigned char *buffer_;
> 
> 	//config values
> 	//topic name of the gazebo message publisher
> 	std::string topic_name_;
> 	double      width_, height_;
> 	//id of the shared memory object
> 	std::string              shm_id_;
> 	std::string              frame_;
> 	firevision::colorspace_t format_from_;
> 	firevision::colorspace_t format_to_;
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-webcam/gazsim_webcam_plugin.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-webcam/gazsim_webcam_plugin.cpp
22,23d21
< #include <core/plugin.h>
< 
25a24,25
> #include <core/plugin.h>
> 
35,37c35,36
<  public:
< 
<   /** Constructor.
---
> public:
> 	/** Constructor.
40,44c39,42
<   GazsimWebcamPlugin(Configuration *config)
<     : Plugin(config)
<   {
<     thread_list.push_back(new WebcamSimThread());
<   }
---
> 	explicit GazsimWebcamPlugin(Configuration *config) : Plugin(config)
> 	{
> 		thread_list.push_back(new WebcamSimThread());
> 	}
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-webcam/gazsim_webcam_thread.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-webcam/gazsim_webcam_thread.cpp
23a24,25
> #include <aspect/logging.h>
> #include <fvutils/color/conversions.h>
25,26d26
< #include <stdio.h>
< #include <math.h>
29d28
< #include <gazebo/transport/Node.hh>
30a30
> #include <gazebo/transport/Node.hh>
32,34c32,33
< #include <aspect/logging.h>
< 
< #include <fvutils/color/conversions.h>
---
> #include <math.h>
> #include <stdio.h>
46,47c45,46
<   : Thread("WebcamSimThread", Thread::OPMODE_WAITFORWAKEUP),
<     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)
---
> : Thread("WebcamSimThread", Thread::OPMODE_WAITFORWAKEUP),
>   BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)
51c50,51
< void WebcamSimThread::init()
---
> void
> WebcamSimThread::init()
53,54c53,54
<   logger->log_debug(name(), "Initializing Simulation of the Webcams");
<   shm_ids_ = config->get_strings("/gazsim/webcam/shm-image-ids");
---
> 	logger->log_debug(name(), "Initializing Simulation of the Webcams");
> 	shm_ids_ = config->get_strings("/gazsim/webcam/shm-image-ids");
56,59c56,58
<   for (std::vector<std::string>::iterator it = shm_ids_.begin(); it != shm_ids_.end(); ++it)
<   {
<     webcams_.push_back(new GazsimWebcam(*it, gazebo_world_node, config));
<   }
---
> 	for (std::vector<std::string>::iterator it = shm_ids_.begin(); it != shm_ids_.end(); ++it) {
> 		webcams_.push_back(new GazsimWebcam(*it, gazebo_world_node, config));
> 	}
62c61,62
< void WebcamSimThread::finalize()
---
> void
> WebcamSimThread::finalize()
64,67c64,66
<   for (std::vector<GazsimWebcam*>::iterator it = webcams_.begin(); it != webcams_.end(); ++it)
<   {
<     delete *it;
<   }  
---
> 	for (std::vector<GazsimWebcam *>::iterator it = webcams_.begin(); it != webcams_.end(); ++it) {
> 		delete *it;
> 	}
70c69,70
< void WebcamSimThread::loop()
---
> void
> WebcamSimThread::loop()
72c72
<   //The interesting stuff happens in the callback of the webcams
---
> 	//The interesting stuff happens in the callback of the webcams
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-webcam/gazsim_webcam_thread.h fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-webcam/gazsim_webcam_thread.h
22,23c22,23
< #ifndef __PLUGINS_GAZSIM_WEBCAM_THREAD_H_
< #define __PLUGINS_GAZSIM_WEBCAM_THREAD_H_
---
> #ifndef _PLUGINS_GAZSIM_WEBCAM_THREAD_H_
> #define _PLUGINS_GAZSIM_WEBCAM_THREAD_H_
25c25,26
< #include <core/threading/thread.h>
---
> #include <aspect/blackboard.h>
> #include <aspect/blocked_timing.h>
29,30c30,31
< #include <aspect/blackboard.h>
< #include <aspect/blocked_timing.h>
---
> #include <core/threading/thread.h>
> #include <fvutils/ipc/shm_image.h>
32d32
< #include <string.h>
34d33
< #include <fvutils/ipc/shm_image.h>
35a35
> #include <string.h>
38c38,39
< #include <gazebo/transport/TransportTypes.hh>
---
> #include "gazsim_webcam.h"
> 
39a41
> #include <gazebo/transport/TransportTypes.hh>
42,51c44,50
< #include "gazsim_webcam.h"
< 
< class WebcamSimThread
< : public fawkes::Thread,
<   public fawkes::ClockAspect,
<   public fawkes::LoggingAspect,
<   public fawkes::ConfigurableAspect,
<   public fawkes::BlackBoardAspect,
<   public fawkes::BlockedTimingAspect,
<   public fawkes::GazeboAspect
---
> class WebcamSimThread : public fawkes::Thread,
>                         public fawkes::ClockAspect,
>                         public fawkes::LoggingAspect,
>                         public fawkes::ConfigurableAspect,
>                         public fawkes::BlackBoardAspect,
>                         public fawkes::BlockedTimingAspect,
>                         public fawkes::GazeboAspect
53,54c52,60
<  public:
<   WebcamSimThread();
---
> public:
> 	WebcamSimThread();
> 
> 	virtual void init();
> 	virtual void loop();
> 	virtual void finalize();
> 
> private:
> 	std::vector<GazsimWebcam *> webcams_;
56,64c62,63
<   virtual void init();
<   virtual void loop();
<   virtual void finalize();
< 
<  private:
<   std::vector<GazsimWebcam*> webcams_;
<   
<   //config values
<   std::vector<std::string> shm_ids_;
---
> 	//config values
> 	std::vector<std::string> shm_ids_;
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/gazsim-webcam/Makefile fawkes-robotino-2016/fawkes/src/plugins/gazebo/gazsim-webcam/Makefile
25c25
< LIBS_gazsim_webcam = fawkescore fawkesutils fawkesaspects fvutils \
---
> LIBS_gazsim_webcam = m fawkescore fawkesutils fawkesaspects fvutils \
30a31
> PLUGINS_all = $(PLUGINDIR)/gazsim-webcam.so
36c37,38
<   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) -lm $(call boost-libs-ldflags,system) -lboost_system
---
>   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) \
>              $(call boost-libs-ldflags,system)
38c40
<   PLUGINS_all = $(PLUGINDIR)/gazsim-webcam.so
---
>   PLUGINS_build = $(PLUGINS_all)
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/Makefile fawkes-robotino-2016/fawkes/src/plugins/gazebo/Makefile
26,27c26,27
< 	  gazsim-vis-localization gazsim-webcam gazsim-timesource \
< 	  gazsim-depthcam
---
>           gazsim-vis-localization gazsim-webcam gazsim-timesource \
>           gazsim-depthcam
29c29
< LIBS_gazebo = fawkescore fawkesaspects fawkesgazeboaspect
---
> LIBS_gazebo = m fawkescore fawkesaspects fawkesgazeboaspect
32a33
> PLUGINS_all = $(PLUGINDIR)/gazebo.$(SOEXT)
36c37,38
<   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) -lm $(call boost-libs-ldflags,system)
---
>   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) \
>              $(call boost-libs-ldflags,system)
38c40
<   PLUGINS_all = $(PLUGINDIR)/gazebo.$(SOEXT)
---
>   PLUGINS_build = $(PLUGINS_all)
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/node_thread.cpp fawkes-robotino-2016/fawkes/src/plugins/gazebo/node_thread.cpp
26,28d25
< #include <gazebo/transport/TransportIface.hh>
< #include <gazebo/transport/TransportTypes.hh>
< #include <gazebo/transport/Node.hh>
31d27
< #include <gazebo/msgs/msgs.hh>
32a29,32
> #include <gazebo/msgs/msgs.hh>
> #include <gazebo/transport/Node.hh>
> #include <gazebo/transport/TransportIface.hh>
> #include <gazebo/transport/TransportTypes.hh>
46,48c46,48
<   : Thread("GazeboNodeThread", Thread::OPMODE_WAITFORWAKEUP),
<     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_POST_LOOP),
<     AspectProviderAspect(&__gazebo_aspect_inifin)
---
> : Thread("GazeboNodeThread", Thread::OPMODE_WAITFORWAKEUP),
>   BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_POST_LOOP),
>   AspectProviderAspect(&gazebo_aspect_inifin_)
52d51
< 
58d56
< 
62,87c60,84
<   //read config values
<   robot_channel = config->get_string("/gazsim/world-name")
<     + "/" + config->get_string("/gazsim/robot-name");
<   
<   world_name = config->get_string("/gazsim/world-name");
< 
<   if(gazebo::transport::is_stopped()) {
<     gazebo::transport::init();
<     gazebo::transport::run();
<   }
<   else {
<     logger->log_warn(name(), "Gazebo already running ");
<   }
< 
<   //Initialize Communication nodes:
<   //the common one for the robot
<   gazebo::transport::NodePtr node(new gazebo::transport::Node());
<   __gazebonode = node;
<   //initialize node (this node only communicates with nodes that were initialized with the same string)
<   __gazebonode->Init(robot_channel.c_str());
<   __gazebo_aspect_inifin.set_gazebonode(__gazebonode);
<   
<   //and the node for world change messages
<   __gazebo_world_node = gazebo::transport::NodePtr(new gazebo::transport::Node());
<   __gazebo_world_node->Init(world_name.c_str());
<   __gazebo_aspect_inifin.set_gazebo_world_node(__gazebo_world_node);
---
> 	//read config values
> 	robot_channel =
> 	  config->get_string("/gazsim/world-name") + "/" + config->get_string("/gazsim/robot-name");
> 
> 	world_name = config->get_string("/gazsim/world-name");
> 
> 	if (gazebo::transport::is_stopped()) {
> 		gazebo::transport::init();
> 		gazebo::transport::run();
> 	} else {
> 		logger->log_warn(name(), "Gazebo already running ");
> 	}
> 
> 	//Initialize Communication nodes:
> 	//the common one for the robot
> 	gazebo::transport::NodePtr node(new gazebo::transport::Node());
> 	gazebonode_ = node;
> 	//initialize node (this node only communicates with nodes that were initialized with the same string)
> 	gazebonode_->Init(robot_channel.c_str());
> 	gazebo_aspect_inifin_.set_gazebonode(gazebonode_);
> 
> 	//and the node for world change messages
> 	gazebo_world_node_ = gazebo::transport::NodePtr(new gazebo::transport::Node());
> 	gazebo_world_node_->Init(world_name.c_str());
> 	gazebo_aspect_inifin_.set_gazebo_world_node(gazebo_world_node_);
90d86
< 
94,99c90,95
<   __gazebonode->Fini();
<   __gazebonode.reset();
<   __gazebo_aspect_inifin.set_gazebonode(__gazebonode);
<   __gazebo_world_node->Fini();
<   __gazebo_world_node.reset();
<   __gazebo_aspect_inifin.set_gazebonode(__gazebo_world_node);
---
> 	gazebonode_->Fini();
> 	gazebonode_.reset();
> 	gazebo_aspect_inifin_.set_gazebonode(gazebonode_);
> 	gazebo_world_node_->Fini();
> 	gazebo_world_node_.reset();
> 	gazebo_aspect_inifin_.set_gazebonode(gazebo_world_node_);
101d96
< 
diff -r fawkes-robotino/fawkes/src/plugins/gazebo/node_thread.h fawkes-robotino-2016/fawkes/src/plugins/gazebo/node_thread.h
23,24c23,24
< #ifndef __PLUGINS_GAZEBO_NODE_THREAD_H_
< #define __PLUGINS_GAZEBO_NODE_THREAD_H_
---
> #ifndef _PLUGINS_GAZEBO_NODE_THREAD_H_
> #define _PLUGINS_GAZEBO_NODE_THREAD_H_
26,30d25
< #include <core/threading/thread.h>
< #include <aspect/logging.h>
< #include <aspect/configurable.h>
< #include <aspect/clock.h>
< #include <aspect/blocked_timing.h>
31a27,31
> #include <aspect/blocked_timing.h>
> #include <aspect/clock.h>
> #include <aspect/configurable.h>
> #include <aspect/logging.h>
> #include <core/threading/thread.h>
32a33
> #include <sys/types.h>
34d34
< #include <string.h>
36c36
< #include <sys/types.h>
---
> #include <string.h>
39,41c39,40
<   namespace transport {
<     class Node;
<   }
---
> namespace transport {
> class Node;
42a42
> } // namespace gazebo
44,50c44,49
< class GazeboNodeThread
< : public fawkes::Thread,
<   public fawkes::BlockedTimingAspect,
<   public fawkes::LoggingAspect,
<   public fawkes::ConfigurableAspect,
<   public fawkes::ClockAspect,
<   public fawkes::AspectProviderAspect
---
> class GazeboNodeThread : public fawkes::Thread,
>                          public fawkes::BlockedTimingAspect,
>                          public fawkes::LoggingAspect,
>                          public fawkes::ConfigurableAspect,
>                          public fawkes::ClockAspect,
>                          public fawkes::AspectProviderAspect
52,69c51,74
<  public:
<   GazeboNodeThread();
<   virtual ~GazeboNodeThread();
< 
<   virtual void init();
<   virtual void loop();
<   virtual void finalize();
< 
<  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
<  protected: virtual void run() { Thread::run(); }
< 
<  private:
<   //Node for communication to gazebo-robot-plugins
<   gazebo::transport::NodePtr  __gazebonode;
<   //Node to control the gazebo world (e.g. spawn visual objects)
<   gazebo::transport::NodePtr  __gazebo_world_node;
<   //Publisher to send Messages:
<   gazebo::transport::PublisherPtr __visual_publisher, __model_publisher, __request_publisher, __light_publisher;
---
> public:
> 	GazeboNodeThread();
> 	virtual ~GazeboNodeThread();
> 
> 	virtual void init();
> 	virtual void loop();
> 	virtual void finalize();
> 
> 	/** Stub to see name in backtrace for easier debugging. @see Thread::run() */
> protected:
> 	virtual void
> 	run()
> 	{
> 		Thread::run();
> 	}
> 
> private:
> 	//Node for communication to gazebo-robot-plugins
> 	gazebo::transport::NodePtr gazebonode_;
> 	//Node to control the gazebo world (e.g. spawn visual objects)
> 	gazebo::transport::NodePtr gazebo_world_node_;
> 	//Publisher to send Messages:
> 	gazebo::transport::PublisherPtr visual_publisher_, model_publisher_, request_publisher_,
> 	  light_publisher_;
71c76
<   fawkes::GazeboAspectIniFin  __gazebo_aspect_inifin;
---
> 	fawkes::GazeboAspectIniFin gazebo_aspect_inifin_;
73,74c78,79
<   //channel of a specified robot for the gazebo node communication
<   std::string robot_channel, world_name;
---
> 	//channel of a specified robot for the gazebo node communication
> 	std::string robot_channel, world_name;
diff -r fawkes-robotino/src/plugins/gazebo/Makefile fawkes-robotino-2016/src/plugins/gazebo/Makefile
21c21
< 	  gazsim-machine-signal gazsim-gripper \
---
> 	  gazsim-gripper \
23a24
> # gazsim-machine-signal

コメントを残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です

*