{"id":2648,"date":"2021-01-08T12:51:32","date_gmt":"2021-01-08T03:51:32","guid":{"rendered":"https:\/\/www.kdel.org\/wp\/?p=2648"},"modified":"2022-05-20T16:16:17","modified_gmt":"2022-05-20T07:16:17","slug":"2648","status":"publish","type":"post","link":"https:\/\/www.kdel.org\/wp\/?p=2648","title":{"rendered":"fawkes-robotino-2016\u306egazebo9\u4ee5\u964d\u3078\u306e\u5bfe\u5fdc"},"content":{"rendered":"

Ubuntu 18.04\u3078\u306efawkes-robotino \u306e\u30a4\u30f3\u30b9\u30c8\u30fc\u30eb\uff08\u8a66\u884c\u932f\u8aa4\u7248\uff09<\/a>\u306b\u3066\uff0c<\/p>\n

gazebo \u95a2\u4fc2\u306fgazebo5\u304b\u3089gazebo9\u307e\u3067\u4e0a\u304c\u3063\u3066\u3044\u3066\uff0cgazebo::physics:World \u306eGetName \u306a\u3069\u306a\u3069\u304c\u306a\u304f\u306a\u3063\u3066\u3044\u308b\u305f\u3081\uff0c\u5bfe\u5fdc\u304c\u9762\u5012\u306a\u306e\u3067\u3044\u3063\u305f\u3093\u30a4\u30f3\u30b9\u30c8\u30fc\u30eb\u30ea\u30b9\u30c8\u304b\u3089\u524a\u9664\u3057\u307e\u3057\u305f\uff0e<\/p><\/blockquote>\n

\u3068\u3042\u308a\u307e\u3057\u305f\u304c\uff0c\u305d\u306e\u90e8\u5206\u306e\u7b54\u3048\u5408\u308f\u305b\u3067\u3059(2016\u30682019\u306ediff)<\/p>\n

\r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/aspect\/gazebo.cpp fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/aspect\/gazebo.cpp\r\n68c64,65\r\n< GazeboAspect::init_GazeboAspect(gazebo::transport::NodePtr gazebonode , gazebo::transport::NodePtr gazebo_world_node)\r\n---\r\n> GazeboAspect::init_GazeboAspect(gazebo::transport::NodePtr gazebonode,\r\n>                                 gazebo::transport::NodePtr gazebo_world_node)\r\n70,71c67,68\r\n<   this->gazebonode = gazebonode;\r\n<   this->gazebo_world_node = gazebo_world_node;\r\n---\r\n> \tthis->gazebonode        = gazebonode;\r\n> \tthis->gazebo_world_node = gazebo_world_node;\r\n80c77\r\n<   gazebonode.reset();\r\n---\r\n> \tgazebonode.reset();\r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/aspect\/gazebo.h fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/aspect\/gazebo.h\r\n24,25c24,25\r\n< #ifndef __PLUGINS_GAZEBO_ASPECT_GAZEBO_H_\r\n< #define __PLUGINS_GAZEBO_ASPECT_GAZEBO_H_\r\n---\r\n> #ifndef _PLUGINS_GAZEBO_ASPECT_GAZEBO_H_\r\n> #define _PLUGINS_GAZEBO_ASPECT_GAZEBO_H_\r\n33,35c33,34\r\n< #if 0 \/* just to make Emacs auto-indent happy *\/\r\n< }\r\n< #endif\r\n---\r\n> \r\n> class GazeboAspectIniFin;\r\n39c38\r\n<   friend class GazeboAspectIniFin;\r\n---\r\n> \tfriend GazeboAspectIniFin;\r\n41,48c40,47\r\n<  public:\r\n<   GazeboAspect();\r\n<   virtual ~GazeboAspect();\r\n< \r\n<  protected:\r\n<   \/\/\/ Gazebo Node for communication with a robot\r\n<   gazebo::transport::NodePtr gazebonode;\r\n<   \/**\r\n---\r\n> public:\r\n> \tGazeboAspect();\r\n> \tvirtual ~GazeboAspect();\r\n> \r\n> protected:\r\n> \t\/\/\/ Gazebo Node for communication with a robot\r\n> \tgazebo::transport::NodePtr gazebonode;\r\n> \t\/**\r\n52c51\r\n<   gazebo::transport::NodePtr gazebo_world_node;\r\n---\r\n> \tgazebo::transport::NodePtr gazebo_world_node;\r\n54,56c53,56\r\n<  private:\r\n<   void init_GazeboAspect(gazebo::transport::NodePtr gazebonode, gazebo::transport::NodePtr gazebo_world_node);\r\n<   void finalize_GazeboAspect();\r\n---\r\n> private:\r\n> \tvoid init_GazeboAspect(gazebo::transport::NodePtr gazebonode,\r\n> \t                       gazebo::transport::NodePtr gazebo_world_node);\r\n> \tvoid finalize_GazeboAspect();\r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/aspect\/gazebo_inifin.cpp fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/aspect\/gazebo_inifin.cpp\r\n24d23\r\n< #include <plugins\/gazebo\/aspect\/gazebo_inifin.h>\r\n25a25,26\r\n> #include <plugins\/gazebo\/aspect\/gazebo_inifin.h>\r\n> \r\n29,31d29\r\n< #if 0 \/* just to make Emacs auto-indent happy *\/\r\n< }\r\n< #endif\r\n41,42c39\r\n< GazeboAspectIniFin::GazeboAspectIniFin()\r\n<   : AspectIniFin("GazeboAspect")\r\n---\r\n> GazeboAspectIniFin::GazeboAspectIniFin() : AspectIniFin("GazeboAspect")\r\n52,61c49,59\r\n<   GazeboAspect *gazebo_thread;\r\n<   gazebo_thread = dynamic_cast<GazeboAspect *>(thread);\r\n<   if (gazebo_thread == NULL) {\r\n<     throw CannotInitializeThreadException("Thread '%s' claims to have the "\r\n< \t\t\t\t\t  "GazeboAspect, but RTTI says it "\r\n< \t\t\t\t\t  "has not. ", thread->name());\r\n<   }\r\n<   if (! __gazebonode) {\r\n<     throw CannotInitializeThreadException("Gazebo node handle has not been set.");\r\n<   }\r\n---\r\n> \tGazeboAspect *gazebo_thread;\r\n> \tgazebo_thread = dynamic_cast<GazeboAspect *>(thread);\r\n> \tif (gazebo_thread == NULL) {\r\n> \t\tthrow CannotInitializeThreadException("Thread '%s' claims to have the "\r\n> \t\t                                      "GazeboAspect, but RTTI says it "\r\n> \t\t                                      "has not. ",\r\n> \t\t                                      thread->name());\r\n> \t}\r\n> \tif (!gazebonode_) {\r\n> \t\tthrow CannotInitializeThreadException("Gazebo node handle has not been set.");\r\n> \t}\r\n63c61\r\n<   gazebo_thread->init_GazeboAspect(__gazebonode, __gazebo_world_node);\r\n---\r\n> \tgazebo_thread->init_GazeboAspect(gazebonode_, gazebo_world_node_);\r\n72,79c70,78\r\n<   GazeboAspect *gazebo_thread;\r\n<   gazebo_thread = dynamic_cast<GazeboAspect *>(thread);\r\n<   if (gazebo_thread == NULL) {\r\n<     throw CannotFinalizeThreadException("Thread '%s' claims to have the "\r\n< \t\t\t\t\t"GazeboAspect, but RTTI says it "\r\n< \t\t\t\t\t"has not. ", thread->name());\r\n<   }\r\n<   gazebo_thread->finalize_GazeboAspect();\r\n---\r\n> \tGazeboAspect *gazebo_thread;\r\n> \tgazebo_thread = dynamic_cast<GazeboAspect *>(thread);\r\n> \tif (gazebo_thread == NULL) {\r\n> \t\tthrow CannotFinalizeThreadException("Thread '%s' claims to have the "\r\n> \t\t                                    "GazeboAspect, but RTTI says it "\r\n> \t\t                                    "has not. ",\r\n> \t\t                                    thread->name());\r\n> \t}\r\n> \tgazebo_thread->finalize_GazeboAspect();\r\n82d80\r\n< \r\n90c88\r\n<   __gazebonode = gazebonode;\r\n---\r\n> \tgazebonode_ = gazebonode;\r\n99c97\r\n<   __gazebo_world_node = gazebo_world_node;\r\n---\r\n> \tgazebo_world_node_ = gazebo_world_node;\r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/aspect\/gazebo_inifin.h fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/aspect\/gazebo_inifin.h\r\n24,25c24,25\r\n< #ifndef __PLUGINS_GAZEBO_ASPECT_GAZEBO_INIFIN_H_\r\n< #define __PLUGINS_GAZEBO_ASPECT_GAZEBO_INIFIN_H_\r\n---\r\n> #ifndef _PLUGINS_GAZEBO_ASPECT_GAZEBO_INIFIN_H_\r\n> #define _PLUGINS_GAZEBO_ASPECT_GAZEBO_INIFIN_H_\r\n34,36d33\r\n< #if 0 \/* just to make Emacs auto-indent happy *\/\r\n< }\r\n< #endif\r\n40,41c37,38\r\n<  public:\r\n<   GazeboAspectIniFin();\r\n---\r\n> public:\r\n> \tGazeboAspectIniFin();\r\n43,52c40,41\r\n<   virtual void init(Thread *thread);\r\n<   virtual void finalize(Thread *thread);\r\n< \r\n<   \/\/setters for the node_thread\r\n<   void set_gazebonode(gazebo::transport::NodePtr gazebonode);\r\n<   void set_gazebo_world_node(gazebo::transport::NodePtr gazebo_world_node);\r\n< \r\n<  private:\r\n<   gazebo::transport::NodePtr __gazebonode;\r\n<   gazebo::transport::NodePtr __gazebo_world_node;\r\n---\r\n> \tvirtual void init(Thread *thread);\r\n> \tvirtual void finalize(Thread *thread);\r\n53a43,49\r\n> \t\/\/setters for the node_thread\r\n> \tvoid set_gazebonode(gazebo::transport::NodePtr gazebonode);\r\n> \tvoid set_gazebo_world_node(gazebo::transport::NodePtr gazebo_world_node);\r\n> \r\n> private:\r\n> \tgazebo::transport::NodePtr gazebonode_;\r\n> \tgazebo::transport::NodePtr gazebo_world_node_;\r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/aspect\/Makefile fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/aspect\/Makefile\r\n30c30,31\r\n< OBJS_all    = $(OBJS_libfawkesgazeboaspect)\r\n---\r\n> OBJS_all = $(OBJS_libfawkesgazeboaspect)\r\n> LIBS_all = $(LIBDIR)\/libfawkesgazeboaspect.so\r\n33c34\r\n<   LIBS_all = $(LIBDIR)\/libfawkesgazeboaspect.so\r\n---\r\n>   LIBS_build = $(LIBS_all)\r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/gazebo.mk fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/gazebo.mk\r\n15a16,17\r\n> include $(BUILDSYSDIR)\/boost.mk\r\n> \r\n25a28\r\n>   HAVE_GAZEBO_96 = $(if $(shell $(PKGCONFIG) --atleast-version=9.6.0 'gazebo'; echo $${?\/1\/}),1,0)\r\n29c32,35\r\n<   CFLAGS_GAZEBO  = -DHAVE_GAZEBO $(shell $(PKGCONFIG) --cflags 'gazebo')\r\n---\r\n>   # Gazebo 8 declared several symbols as deprecated but still uses them.\r\n>   # Disable the deprecated declarations warning until this is fixed.\r\n>   CFLAGS_GAZEBO  = -DHAVE_GAZEBO $(shell $(PKGCONFIG) --cflags 'gazebo') \\\r\n>                    -Wno-deprecated-declarations\r\n30a37,40\r\n> \r\n>   ifeq ($(HAVE_GAZEBO_96)$(boost-have-lib system),11)\r\n>     LDFLAGS_GAZEBO += $(boost-lib-ldflags system)\r\n>   endif\r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/gazebo_plugin.cpp fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/gazebo_plugin.cpp\r\n26d25\r\n< #include <gazebo\/transport\/TransportIface.hh>\r\n28a28,29\r\n> #include <gazebo\/transport\/TransportIface.hh>\r\n> \r\n41,42c42\r\n< GazeboPlugin::GazeboPlugin(Configuration *config)\r\n<   : Plugin(config)\r\n---\r\n> GazeboPlugin::GazeboPlugin(Configuration *config) : Plugin(config)\r\n44c44\r\n<   thread_list.push_back(new GazeboNodeThread());\r\n---\r\n> \tthread_list.push_back(new GazeboNodeThread());\r\n46d45\r\n< \r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/gazebo_plugin.h fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/gazebo_plugin.h\r\n23,24c23,24\r\n< #ifndef __PLUGINS_GAZEBO_GAZEBO_PLUGIN_H_\r\n< #define __PLUGINS_GAZEBO_GAZEBO_PLUGIN_H_\r\n---\r\n> #ifndef _PLUGINS_GAZEBO_GAZEBO_PLUGIN_H_\r\n> #define _PLUGINS_GAZEBO_GAZEBO_PLUGIN_H_\r\n30,31c30,31\r\n<  public:\r\n<   GazeboPlugin(fawkes::Configuration *config);\r\n---\r\n> public:\r\n> \texplicit GazeboPlugin(fawkes::Configuration *config);\r\ndiff -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\r\n25a26\r\n> \r\n35,36c36,37\r\n<  public:\r\n<   \/** Constructor.\r\n---\r\n> public:\r\n> \t\/** Constructor.\r\n39,42c40,43\r\n<   GazsimCommPlugin(Configuration *config) : Plugin(config)\r\n<   {\r\n<     thread_list.push_back(new GazsimCommThread());\r\n<   }\r\n---\r\n> \texplicit GazsimCommPlugin(Configuration *config) : Plugin(config)\r\n> \t{\r\n> \t\tthread_list.push_back(new GazsimCommThread());\r\n> \t}\r\n44d44\r\n< \r\ndiff -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\r\n24a25,26\r\n> #include "gazsim_comm_thread.h"\r\n> \r\n26d27\r\n< #include <protobuf_comm\/peer.h>\r\n28,29c29,30\r\n< #include <stdlib.h>\r\n< #include "gazsim_comm_thread.h"\r\n---\r\n> #include <protobuf_comm\/peer.h>\r\n> \r\n30a32\r\n> #include <stdlib.h>\r\n41,42c43,44\r\n<   : Thread("GazsimCommThread", Thread::OPMODE_WAITFORWAKEUP),\r\n<     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE)\r\n---\r\n> : Thread("GazsimCommThread", Thread::OPMODE_WAITFORWAKEUP),\r\n>   BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE)\r\n50d51\r\n< \r\n54,55c55,56\r\n<   \/\/logger->log_info(name(), "GazsimComm initializing");\r\n<   initialized_ = false;\r\n---\r\n> \t\/\/logger->log_info(name(), "GazsimComm initializing");\r\n> \tinitialized_ = false;\r\n57,128c58,136\r\n<   \/\/read config values\r\n<   proto_dirs_ = config->get_strings("\/gazsim\/proto-dirs");\r\n<   package_loss_ = config->get_float("\/gazsim\/comm\/package-loss");\r\n<   addresses_ = config->get_strings("\/gazsim\/comm\/addresses");\r\n<   send_ports_ = config->get_uints("\/gazsim\/comm\/send-ports");\r\n<   recv_ports_ = config->get_uints("\/gazsim\/comm\/recv-ports");\r\n<   use_crypto1_ = config->get_bool("\/gazsim\/comm\/use-crypto1");\r\n<   use_crypto2_ = config->get_bool("\/gazsim\/comm\/use-crypto1");\r\n<   send_ports_crypto1_ = config->get_uints("\/gazsim\/comm\/send-ports-crypto1");\r\n<   recv_ports_crypto1_ = config->get_uints("\/gazsim\/comm\/recv-ports-crypto1");\r\n<   send_ports_crypto2_ = config->get_uints("\/gazsim\/comm\/send-ports-crypto2");\r\n<   recv_ports_crypto2_ = config->get_uints("\/gazsim\/comm\/recv-ports-crypto2");\r\n<   if(addresses_.size() != send_ports_.size() || addresses_.size() != recv_ports_.size()\r\n<      || ( use_crypto1_ && addresses_.size() != send_ports_crypto1_.size())\r\n<      || ( use_crypto1_ && addresses_.size() != recv_ports_crypto1_.size())\r\n<      || ( use_crypto2_ && addresses_.size() != send_ports_crypto2_.size())\r\n<      || ( use_crypto2_ && addresses_.size() != recv_ports_crypto2_.size()))\r\n<   {\r\n<     logger->log_warn(name(), "\/gazsim\/comm\/ has an invalid configuration!");\r\n<   }\r\n< \r\n< \r\n< \r\n<   \/\/resolve proto paths\r\n<   try {\r\n<     proto_dirs_ = config->get_strings("\/clips-protobuf\/proto-dirs");\r\n<     for (size_t i = 0; i < proto_dirs_.size(); ++i) {\r\n<       std::string::size_type pos;\r\n<       if ((pos = proto_dirs_[i].find("@BASEDIR@")) != std::string::npos) {\r\n< \tproto_dirs_[i].replace(pos, 9, BASEDIR);\r\n<       }\r\n<       if ((pos = proto_dirs_[i].find("@FAWKES_BASEDIR@")) != std::string::npos) {\r\n< \tproto_dirs_[i].replace(pos, 16, FAWKES_BASEDIR);\r\n<       }\r\n<       if ((pos = proto_dirs_[i].find("@RESDIR@")) != std::string::npos) {\r\n< \tproto_dirs_[i].replace(pos, 8, RESDIR);\r\n<       }\r\n<       if ((pos = proto_dirs_[i].find("@CONFDIR@")) != std::string::npos) {\r\n< \tproto_dirs_[i].replace(pos, 9, CONFDIR);\r\n<       }\r\n<       if (proto_dirs_[i][proto_dirs_.size()-1] != '\/') {\r\n< \tproto_dirs_[i] += "\/";\r\n<       }\r\n<     }\r\n<   } catch (Exception &e) {\r\n<     logger->log_warn(name(), "Failed to load proto paths from config, exception follows");\r\n<     logger->log_warn(name(), e);\r\n<   }\r\n< \r\n<   \/\/create peer connections \r\n<   peers_.resize(addresses_.size());\r\n<   peers_crypto1_.resize(addresses_.size());\r\n<   peers_crypto2_.resize(addresses_.size());\r\n<   for(unsigned int i = 0; i < addresses_.size(); i++)\r\n<   {\r\n<     peers_[i] = new ProtobufBroadcastPeer(addresses_[i], send_ports_[i],\r\n< \t\t\t\t\t  recv_ports_[i], proto_dirs_);\r\n<     peers_[i]->signal_received_raw().connect(boost::bind(&GazsimCommThread::receive_raw_msg, this, _1, _2, _3, _4));\r\n<     if(use_crypto1_)\r\n<     {\r\n<       peers_crypto1_[i] = new ProtobufBroadcastPeer(addresses_[i], send_ports_crypto1_[i],\r\n< \t\t\t\t\t  recv_ports_crypto1_[i], proto_dirs_);\r\n<       peers_crypto1_[i]->signal_received_raw().connect(boost::bind(&GazsimCommThread::receive_raw_msg, this, _1, _2, _3, _4));\r\n<     }\r\n<     if(use_crypto2_)\r\n<     {\r\n<       peers_crypto2_[i] = new ProtobufBroadcastPeer(addresses_[i], send_ports_crypto2_[i],\r\n< \t\t\t\t\t\t    recv_ports_crypto2_[i], proto_dirs_);\r\n<       peers_crypto2_[i]->signal_received_raw().connect(boost::bind(&GazsimCommThread::receive_raw_msg, this, _1, _2, _3, _4));\r\n<     }\r\n<   }\r\n<   initialized_ = true;\r\n---\r\n> \t\/\/read config values\r\n> \tproto_dirs_         = config->get_strings("\/gazsim\/proto-dirs");\r\n> \tpackage_loss_       = config->get_float("\/gazsim\/comm\/package-loss");\r\n> \taddresses_          = config->get_strings("\/gazsim\/comm\/addresses");\r\n> \tsend_ports_         = config->get_uints("\/gazsim\/comm\/send-ports");\r\n> \trecv_ports_         = config->get_uints("\/gazsim\/comm\/recv-ports");\r\n> \tuse_crypto1_        = config->get_bool("\/gazsim\/comm\/use-crypto1");\r\n> \tuse_crypto2_        = config->get_bool("\/gazsim\/comm\/use-crypto1");\r\n> \tsend_ports_crypto1_ = config->get_uints("\/gazsim\/comm\/send-ports-crypto1");\r\n> \trecv_ports_crypto1_ = config->get_uints("\/gazsim\/comm\/recv-ports-crypto1");\r\n> \tsend_ports_crypto2_ = config->get_uints("\/gazsim\/comm\/send-ports-crypto2");\r\n> \trecv_ports_crypto2_ = config->get_uints("\/gazsim\/comm\/recv-ports-crypto2");\r\n> \tif (addresses_.size() != send_ports_.size() || addresses_.size() != recv_ports_.size()\r\n> \t    || (use_crypto1_ && addresses_.size() != send_ports_crypto1_.size())\r\n> \t    || (use_crypto1_ && addresses_.size() != recv_ports_crypto1_.size())\r\n> \t    || (use_crypto2_ && addresses_.size() != send_ports_crypto2_.size())\r\n> \t    || (use_crypto2_ && addresses_.size() != recv_ports_crypto2_.size())) {\r\n> \t\tlogger->log_warn(name(), "\/gazsim\/comm\/ has an invalid configuration!");\r\n> \t}\r\n> \r\n> \t\/\/resolve proto paths\r\n> \ttry {\r\n> \t\tproto_dirs_ = config->get_strings("\/clips-protobuf\/proto-dirs");\r\n> \t\tfor (size_t i = 0; i < proto_dirs_.size(); ++i) {\r\n> \t\t\tstd::string::size_type pos;\r\n> \t\t\tif ((pos = proto_dirs_[i].find("@BASEDIR@")) != std::string::npos) {\r\n> \t\t\t\tproto_dirs_[i].replace(pos, 9, BASEDIR);\r\n> \t\t\t}\r\n> \t\t\tif ((pos = proto_dirs_[i].find("@FAWKES_BASEDIR@")) != std::string::npos) {\r\n> \t\t\t\tproto_dirs_[i].replace(pos, 16, FAWKES_BASEDIR);\r\n> \t\t\t}\r\n> \t\t\tif ((pos = proto_dirs_[i].find("@RESDIR@")) != std::string::npos) {\r\n> \t\t\t\tproto_dirs_[i].replace(pos, 8, RESDIR);\r\n> \t\t\t}\r\n> \t\t\tif ((pos = proto_dirs_[i].find("@CONFDIR@")) != std::string::npos) {\r\n> \t\t\t\tproto_dirs_[i].replace(pos, 9, CONFDIR);\r\n> \t\t\t}\r\n> \t\t\tif (proto_dirs_[i][proto_dirs_.size() - 1] != '\/') {\r\n> \t\t\t\tproto_dirs_[i] += "\/";\r\n> \t\t\t}\r\n> \t\t}\r\n> \t} catch (Exception &e) {\r\n> \t\tlogger->log_warn(name(), "Failed to load proto paths from config, exception follows");\r\n> \t\tlogger->log_warn(name(), e);\r\n> \t}\r\n> \r\n> \t\/\/create peer connections\r\n> \tpeers_.resize(addresses_.size());\r\n> \tpeers_crypto1_.resize(addresses_.size());\r\n> \tpeers_crypto2_.resize(addresses_.size());\r\n> \tfor (unsigned int i = 0; i < addresses_.size(); i++) {\r\n> \t\tpeers_[i] =\r\n> \t\t  new ProtobufBroadcastPeer(addresses_[i], send_ports_[i], recv_ports_[i], proto_dirs_);\r\n> \t\tpeers_[i]->signal_received_raw().connect(\r\n> \t\t  boost::bind(&GazsimCommThread::receive_raw_msg, this, _1, _2, _3, _4));\r\n> \t\tpeers_[i]->signal_send_error().connect(\r\n> \t\t  boost::bind(&GazsimCommThread::peer_send_error, this, addresses_[i], send_ports_[i], _1));\r\n> \t\tif (use_crypto1_) {\r\n> \t\t\tpeers_crypto1_[i] = new ProtobufBroadcastPeer(addresses_[i],\r\n> \t\t\t                                              send_ports_crypto1_[i],\r\n> \t\t\t                                              recv_ports_crypto1_[i],\r\n> \t\t\t                                              proto_dirs_);\r\n> \t\t\tpeers_crypto1_[i]->signal_received_raw().connect(\r\n> \t\t\t  boost::bind(&GazsimCommThread::receive_raw_msg, this, _1, _2, _3, _4));\r\n> \t\t\tpeers_crypto1_[i]->signal_send_error().connect(boost::bind(\r\n> \t\t\t  &GazsimCommThread::peer_send_error, this, addresses_[i], send_ports_crypto1_[i], _1));\r\n> \t\t}\r\n> \t\tif (use_crypto2_) {\r\n> \t\t\tpeers_crypto2_[i] = new ProtobufBroadcastPeer(addresses_[i],\r\n> \t\t\t                                              send_ports_crypto2_[i],\r\n> \t\t\t                                              recv_ports_crypto2_[i],\r\n> \t\t\t                                              proto_dirs_);\r\n> \t\t\tpeers_crypto2_[i]->signal_received_raw().connect(\r\n> \t\t\t  boost::bind(&GazsimCommThread::receive_raw_msg, this, _1, _2, _3, _4));\r\n> \t\t\tpeers_crypto2_[i]->signal_send_error().connect(boost::bind(\r\n> \t\t\t  &GazsimCommThread::peer_send_error, this, addresses_[i], send_ports_crypto2_[i], _1));\r\n> \t\t}\r\n> \t}\r\n> \tinitialized_ = true;\r\n131d138\r\n< \r\n135,138c142,144\r\n<   for(unsigned int i = 0; i < peers_.size(); i++)\r\n<   {\r\n<     delete peers_[i];\r\n<   }\r\n---\r\n> \tfor (unsigned int i = 0; i < peers_.size(); i++) {\r\n> \t\tdelete peers_[i];\r\n> \t}\r\n141d146\r\n< \r\n148,183d152\r\n<  * Receive and forward msg\r\n<  * @param endpoint port msg received from\r\n<  * @param component_id message_component_id\r\n<  * @param msg_type msg_type\r\n<  * @param msg Message\r\n<  *\/\r\n< void\r\n< GazsimCommThread::receive_msg(boost::asio::ip::udp::endpoint &endpoint,\r\n< \t\t       uint16_t component_id, uint16_t msg_type,\r\n< \t\t       std::shared_ptr<google::protobuf::Message> msg)\r\n< {\r\n<   \/\/logger->log_info(name(), "Got Peer Message from port %d", endpoint.port());\r\n<   unsigned int incoming_peer_port = endpoint.port(); \/\/this is suprisingly the send-port\r\n<  \r\n<   if(!initialized_)\r\n<   {\r\n<     return;\r\n<   }\r\n< \r\n<   \/\/simulate package loss\r\n<   double rnd = ((double) rand()) \/ ((double) RAND_MAX); \/\/0.0 <= rnd <= 1.0\r\n<   if(rnd < package_loss_)\r\n<   {\r\n<     return;\r\n<   }\r\n<   \/\/send message to all other peers\r\n<   for(unsigned int i = 0; i < peers_.size(); i++)\r\n<   {\r\n<     if(send_ports_[i] != incoming_peer_port)\r\n<     {\r\n<       peers_[i]->send(msg);\r\n<     }\r\n<   }\r\n< }\r\n< \r\n< \/**\r\n192,193c161,209\r\n< \t\t\t\t  protobuf_comm::frame_header_t &header, void * data,\r\n< \t\t\t\t  size_t length)\r\n---\r\n>                                   protobuf_comm::frame_header_t & header,\r\n>                                   void *                          data,\r\n>                                   size_t                          length)\r\n> {\r\n> \t\/\/logger->log_info(name(), "Got raw Message from port %d", endpoint.port());\r\n> \tunsigned int incoming_peer_port = endpoint.port(); \/\/this is suprisingly the send-port\r\n> \r\n> \tif (!initialized_) {\r\n> \t\treturn;\r\n> \t}\r\n> \r\n> \t\/\/simulate package loss\r\n> \tdouble rnd = ((double)rand()) \/ ((double)RAND_MAX); \/\/0.0 <= rnd <= 1.0\r\n> \tif (rnd < package_loss_) {\r\n> \t\treturn;\r\n> \t}\r\n> \r\n> \t\/\/check which set of peers the message comes from\r\n> \tstd::vector<protobuf_comm::ProtobufBroadcastPeer *> peers;\r\n> \tstd::vector<unsigned int>                           send_ports;\r\n> \tif (std::find(send_ports_.begin(), send_ports_.end(), incoming_peer_port) != send_ports_.end()) {\r\n> \t\tpeers      = peers_;\r\n> \t\tsend_ports = send_ports_;\r\n> \t} else if (use_crypto1_\r\n> \t           && std::find(send_ports_crypto1_.begin(),\r\n> \t                        send_ports_crypto1_.end(),\r\n> \t                        incoming_peer_port)\r\n> \t                != send_ports_crypto1_.end()) {\r\n> \t\tpeers      = peers_crypto1_;\r\n> \t\tsend_ports = send_ports_crypto1_;\r\n> \t} else if (use_crypto2_\r\n> \t           && std::find(send_ports_crypto2_.begin(),\r\n> \t                        send_ports_crypto2_.end(),\r\n> \t                        incoming_peer_port)\r\n> \t                != send_ports_crypto2_.end()) {\r\n> \t\tpeers      = peers_crypto2_;\r\n> \t\tsend_ports = send_ports_crypto2_;\r\n> \t}\r\n> \r\n> \t\/\/send message to all other peers\r\n> \tfor (unsigned int i = 0; i < peers.size(); i++) {\r\n> \t\tif (send_ports[i] != incoming_peer_port) {\r\n> \t\t\tpeers[i]->send_raw(header, data, length);\r\n> \t\t}\r\n> \t}\r\n> }\r\n> \r\n> void\r\n> GazsimCommThread::peer_send_error(std::string address, unsigned int port, std::string err)\r\n195,236c211\r\n<   \/\/logger->log_info(name(), "Got raw Message from port %d", endpoint.port());\r\n<   unsigned int incoming_peer_port = endpoint.port(); \/\/this is suprisingly the send-port\r\n<  \r\n<   if(!initialized_)\r\n<   {\r\n<     return;\r\n<   }\r\n< \r\n<   \/\/simulate package loss\r\n<   double rnd = ((double) rand()) \/ ((double) RAND_MAX); \/\/0.0 <= rnd <= 1.0\r\n<   if(rnd < package_loss_)\r\n<   {\r\n<     return;\r\n<   }\r\n< \r\n<   \/\/check which set of peers the message comes from\r\n<   std::vector<protobuf_comm::ProtobufBroadcastPeer*> peers;\r\n<   std::vector<unsigned int> send_ports;\r\n<   if(std::find(send_ports_.begin(), send_ports_.end(), incoming_peer_port) != send_ports_.end())\r\n<   {\r\n<     peers = peers_;\r\n<     send_ports = send_ports_;\r\n<   }\r\n<   else if(use_crypto1_ && std::find(send_ports_crypto1_.begin(), send_ports_crypto1_.end(), incoming_peer_port) != send_ports_crypto1_.end())\r\n<   {\r\n<     peers = peers_crypto1_;\r\n<     send_ports = send_ports_crypto1_;\r\n<   }  \r\n<   else if(use_crypto2_ && std::find(send_ports_crypto2_.begin(), send_ports_crypto2_.end(), incoming_peer_port) != send_ports_crypto1_.end())\r\n<   {\r\n<     peers = peers_crypto2_;\r\n<     send_ports = send_ports_crypto2_;\r\n<   }\r\n< \r\n<   \/\/send message to all other peers\r\n<   for(unsigned int i = 0; i < peers.size(); i++)\r\n<   {\r\n<     if(send_ports[i] != incoming_peer_port)\r\n<     {\r\n<       peers[i]->send_raw(header, data, length);\r\n<     }\r\n<   }\r\n---\r\n> \tlogger->log_warn(name(), "Peer send error for %s:%u: %s", address.c_str(), port, err.c_str());\r\ndiff -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\r\n25,26c25,26\r\n< #ifndef __PLUGINS_GAZSIM_COMM_COMM_THREAD_H_\r\n< #define __PLUGINS_GAZSIM_COMM_COMM_THREAD_H_\r\n---\r\n> #ifndef _PLUGINS_GAZSIM_COMM_COMM_THREAD_H_\r\n> #define _PLUGINS_GAZSIM_COMM_COMM_THREAD_H_\r\n28,30d27\r\n< #include <core\/threading\/thread.h>\r\n< #include <aspect\/logging.h>\r\n< #include <aspect\/configurable.h>\r\n32c29,31\r\n< #include <boost\/asio.hpp>\r\n---\r\n> #include <aspect\/configurable.h>\r\n> #include <aspect\/logging.h>\r\n> #include <core\/threading\/thread.h>\r\n34d32\r\n< #include <protobuf_comm\/peer.h>\r\n36c34\r\n< #include <list>\r\n---\r\n> #include <protobuf_comm\/peer.h>\r\n37a36,37\r\n> #include <boost\/asio.hpp>\r\n> #include <list>\r\n40c40\r\n<   class ProtobufStreamClient;\r\n---\r\n> class ProtobufStreamClient;\r\n43,47c43,46\r\n< class GazsimCommThread\r\n< : public fawkes::Thread,\r\n<   public fawkes::BlockedTimingAspect,\r\n<   public fawkes::ConfigurableAspect,\r\n<   public fawkes::LoggingAspect\r\n---\r\n> class GazsimCommThread : public fawkes::Thread,\r\n>                          public fawkes::BlockedTimingAspect,\r\n>                          public fawkes::ConfigurableAspect,\r\n>                          public fawkes::LoggingAspect\r\n49,81c48,85\r\n<  public:\r\n<   GazsimCommThread();\r\n<   ~GazsimCommThread();\r\n< \r\n<   virtual void init();\r\n<   virtual void loop();\r\n<   virtual void finalize();\r\n< \r\n<   void receive_msg(boost::asio::ip::udp::endpoint &endpoint,\r\n< \t\t       uint16_t component_id, uint16_t msg_type,\r\n< \t\t       std::shared_ptr<google::protobuf::Message> msg);\r\n<   void receive_raw_msg(boost::asio::ip::udp::endpoint &endpoint,\r\n< \t\t       protobuf_comm::frame_header_t &header, void * data,\r\n< \t\t       size_t length);\r\n< \r\n<  \/** Stub to see name in backtrace for easier debugging. @see Thread::run() *\/\r\n<  protected: virtual void run() { Thread::run(); }\r\n< \r\n<  private:\r\n<   std::vector<protobuf_comm::ProtobufBroadcastPeer*> peers_;\r\n<   std::vector<protobuf_comm::ProtobufBroadcastPeer*> peers_crypto1_;\r\n<   std::vector<protobuf_comm::ProtobufBroadcastPeer*> peers_crypto2_;\r\n< \r\n<   \/\/config values\r\n<   std::vector<std::string> addresses_;\r\n<   std::vector<unsigned int> send_ports_;\r\n<   std::vector<unsigned int> recv_ports_;\r\n<   std::vector<unsigned int> send_ports_crypto1_;\r\n<   std::vector<unsigned int> recv_ports_crypto1_;\r\n<   std::vector<unsigned int> send_ports_crypto2_;\r\n<   std::vector<unsigned int> recv_ports_crypto2_;\r\n<   \r\n<   bool use_crypto1_, use_crypto2_;\r\n---\r\n> public:\r\n> \tGazsimCommThread();\r\n> \t~GazsimCommThread();\r\n> \r\n> \tvirtual void init();\r\n> \tvirtual void loop();\r\n> \tvirtual void finalize();\r\n> \r\n> \t\/** Stub to see name in backtrace for easier debugging. @see Thread::run() *\/\r\n> protected:\r\n> \tvirtual void\r\n> \trun()\r\n> \t{\r\n> \t\tThread::run();\r\n> \t}\r\n> \r\n> private:\r\n> \tvoid peer_send_error(std::string address, unsigned int port, std::string err);\r\n> \tvoid receive_raw_msg(boost::asio::ip::udp::endpoint &endpoint,\r\n> \t                     protobuf_comm::frame_header_t & header,\r\n> \t                     void *                          data,\r\n> \t                     size_t                          length);\r\n> \r\n> private:\r\n> \tstd::vector<protobuf_comm::ProtobufBroadcastPeer *> peers_;\r\n> \tstd::vector<protobuf_comm::ProtobufBroadcastPeer *> peers_crypto1_;\r\n> \tstd::vector<protobuf_comm::ProtobufBroadcastPeer *> peers_crypto2_;\r\n> \r\n> \t\/\/config values\r\n> \tstd::vector<std::string>  addresses_;\r\n> \tstd::vector<unsigned int> send_ports_;\r\n> \tstd::vector<unsigned int> recv_ports_;\r\n> \tstd::vector<unsigned int> send_ports_crypto1_;\r\n> \tstd::vector<unsigned int> recv_ports_crypto1_;\r\n> \tstd::vector<unsigned int> send_ports_crypto2_;\r\n> \tstd::vector<unsigned int> recv_ports_crypto2_;\r\n> \r\n> \tbool use_crypto1_, use_crypto2_;\r\n83,84c87,88\r\n<   std::vector<std::string> proto_dirs_;\r\n<   double package_loss_;\r\n---\r\n> \tstd::vector<std::string> proto_dirs_;\r\n> \tdouble                   package_loss_;\r\n86,87c90,91\r\n<   \/\/helper variables\r\n<   bool initialized_;\r\n---\r\n> \t\/\/helper variables\r\n> \tbool initialized_;\r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/gazsim-comm\/Makefile fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/gazsim-comm\/Makefile\r\n28c28,29\r\n< LIBS_gazsim_comm = fawkescore fawkesutils fawkesaspects fawkes_protobuf_comm fawkesgazeboaspect gazsim_msgs \r\n---\r\n> LIBS_gazsim_comm = fawkescore fawkesutils fawkesaspects fawkes_protobuf_comm \\\r\n>                    fawkesgazeboaspect gazsim_msgs \r\n31c32,33\r\n< OBJS_all = $(OBJS_gazsim_comm)\r\n---\r\n> OBJS_all    = $(OBJS_gazsim_comm)\r\n> PLUGINS_all = $(PLUGINDIR)\/gazsim-comm.so\r\n42c44\r\n<   PLUGINS_all = $(PLUGINDIR)\/gazsim-comm.so\r\n---\r\n>   PLUGINS_build = $(PLUGINS_all)\r\ndiff -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\r\n22,23d21\r\n< #include <core\/plugin.h>\r\n< \r\n25a24,25\r\n> #include <core\/plugin.h>\r\n> \r\n35,37c35,36\r\n<  public:\r\n< \r\n<   \/** Constructor.\r\n---\r\n> public:\r\n> \t\/** Constructor.\r\n40,44c39,42\r\n<   GazsimDepthcamPlugin(Configuration *config)\r\n<     : Plugin(config)\r\n<   {\r\n<     thread_list.push_back(new DepthcamSimThread());\r\n<   }\r\n---\r\n> \texplicit GazsimDepthcamPlugin(Configuration *config) : Plugin(config)\r\n> \t{\r\n> \t\tthread_list.push_back(new DepthcamSimThread());\r\n> \t}\r\ndiff -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\r\n23a24\r\n> #include <aspect\/logging.h>\r\n25,26d25\r\n< #include <stdio.h>\r\n< #include <math.h>\r\n29d27\r\n< #include <gazebo\/transport\/Node.hh>\r\n30a29\r\n> #include <gazebo\/transport\/Node.hh>\r\n32c31,32\r\n< #include <aspect\/logging.h>\r\n---\r\n> #include <math.h>\r\n> #include <stdio.h>\r\n44,45c44,45\r\n<   : Thread("DepthcamSimThread", Thread::OPMODE_WAITFORWAKEUP),\r\n<     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)\r\n---\r\n> : Thread("DepthcamSimThread", Thread::OPMODE_WAITFORWAKEUP),\r\n>   BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)\r\n49c49,50\r\n< void DepthcamSimThread::init()\r\n---\r\n> void\r\n> DepthcamSimThread::init()\r\n51,66c52,68\r\n<   logger->log_debug(name(), "Initializing Simulation of the Depthcam");\r\n<   topic_name_ = config->get_string("\/gazsim\/depthcam\/topic");\r\n<   width_ = config->get_float("\/gazsim\/depthcam\/width");\r\n<   height_ = config->get_float("\/gazsim\/depthcam\/height");\r\n<   frame_ = config->get_string("\/gazsim\/depthcam\/frame");\r\n<   pcl_id_ = config->get_string("\/gazsim\/depthcam\/pointcloud-id");\r\n< \r\n<   depthcam_sub_ = gazebo_world_node->Subscribe(topic_name_, &DepthcamSimThread::on_depthcam_data_msg, this);\r\n< \r\n<   \/\/create pointcloud:\r\n<   pcl_ = new pcl::PointCloud<pcl::PointXYZ>();\r\n<   pcl_->is_dense = false;\r\n<   pcl_->width    = width_;\r\n<   pcl_->height   = height_;\r\n<   pcl_->points.resize(width_ * height_);\r\n<   pcl_->header.frame_id = frame_;\r\n---\r\n> \tlogger->log_debug(name(), "Initializing Simulation of the Depthcam");\r\n> \ttopic_name_ = config->get_string("\/gazsim\/depthcam\/topic");\r\n> \twidth_      = config->get_float("\/gazsim\/depthcam\/width");\r\n> \theight_     = config->get_float("\/gazsim\/depthcam\/height");\r\n> \tframe_      = config->get_string("\/gazsim\/depthcam\/frame");\r\n> \tpcl_id_     = config->get_string("\/gazsim\/depthcam\/pointcloud-id");\r\n> \r\n> \tdepthcam_sub_ =\r\n> \t  gazebo_world_node->Subscribe(topic_name_, &DepthcamSimThread::on_depthcam_data_msg, this);\r\n> \r\n> \t\/\/create pointcloud:\r\n> \tpcl_           = new pcl::PointCloud<pcl::PointXYZ>();\r\n> \tpcl_->is_dense = false;\r\n> \tpcl_->width    = width_;\r\n> \tpcl_->height   = height_;\r\n> \tpcl_->points.resize((size_t)width_ * (size_t)height_);\r\n> \tpcl_->header.frame_id = frame_;\r\n68c70\r\n<   pcl_manager->add_pointcloud(pcl_id_.c_str(), pcl_);\r\n---\r\n> \tpcl_manager->add_pointcloud(pcl_id_.c_str(), pcl_);\r\n71c73,74\r\n< void DepthcamSimThread::finalize()\r\n---\r\n> void\r\n> DepthcamSimThread::finalize()\r\n73c76\r\n<   pcl_manager->remove_pointcloud(pcl_id_.c_str());\r\n---\r\n> \tpcl_manager->remove_pointcloud(pcl_id_.c_str());\r\n76c79,80\r\n< void DepthcamSimThread::loop()\r\n---\r\n> void\r\n> DepthcamSimThread::loop()\r\n78c82\r\n<   \/\/The interesting stuff happens in the callback of the depthcam\r\n---\r\n> \t\/\/The interesting stuff happens in the callback of the depthcam\r\n81,104c85,109\r\n< void DepthcamSimThread::on_depthcam_data_msg(ConstPointCloudPtr &msg)\r\n< {  \r\n<   \/\/ logger->log_info(name(), "Got Point Cloud!");\r\n< \r\n<   \/\/only write when pcl is used\r\n<   \/\/ if (pcl_.use_count() > 1)\r\n<   \/\/ {\r\n<     fawkes::Time capture_time = clock->now();\r\n< \r\n<     pcl::PointCloud<pcl::PointXYZ> &pcl = **pcl_;\r\n<     pcl.header.seq += 1;\r\n<     pcl_utils::set_time(pcl_, capture_time);\r\n< \r\n<     \/\/insert or update points in pointcloud\r\n<     unsigned int idx = 0;\r\n<     for (unsigned int h = 0; h < height_; ++h) {\r\n<       for (unsigned int w = 0; w < width_; ++w, ++idx) {\r\n<         \/\/ Fill in XYZ\r\n<         pcl.points[idx].x = msg->points(idx).z();\r\n<         pcl.points[idx].y = -msg->points(idx).x();\r\n<         pcl.points[idx].z = msg->points(idx).y();\r\n<       }\r\n<     }\r\n<   \/\/ }\r\n---\r\n> void\r\n> DepthcamSimThread::on_depthcam_data_msg(ConstPointCloudPtr &msg)\r\n> {\r\n> \t\/\/ logger->log_info(name(), "Got Point Cloud!");\r\n> \r\n> \t\/\/only write when pcl is used\r\n> \t\/\/ if (pcl_.use_count() > 1)\r\n> \t\/\/ {\r\n> \tfawkes::Time capture_time = clock->now();\r\n> \r\n> \tpcl::PointCloud<pcl::PointXYZ> &pcl = **pcl_;\r\n> \tpcl.header.seq += 1;\r\n> \tpcl_utils::set_time(pcl_, capture_time);\r\n> \r\n> \t\/\/insert or update points in pointcloud\r\n> \tunsigned int idx = 0;\r\n> \tfor (unsigned int h = 0; h < height_; ++h) {\r\n> \t\tfor (unsigned int w = 0; w < width_; ++w, ++idx) {\r\n> \t\t\t\/\/ Fill in XYZ\r\n> \t\t\tpcl.points[idx].x = msg->points(idx).z();\r\n> \t\t\tpcl.points[idx].y = -msg->points(idx).x();\r\n> \t\t\tpcl.points[idx].z = msg->points(idx).y();\r\n> \t\t}\r\n> \t}\r\n> \t\/\/ }\r\ndiff -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\r\n22,23c22,23\r\n< #ifndef __PLUGINS_GAZSIM_DEPTHCAM_THREAD_H_\r\n< #define __PLUGINS_GAZSIM_DEPTHCAM_THREAD_H_\r\n---\r\n> #ifndef _PLUGINS_GAZSIM_DEPTHCAM_THREAD_H_\r\n> #define _PLUGINS_GAZSIM_DEPTHCAM_THREAD_H_\r\n25c25,26\r\n< #include <core\/threading\/thread.h>\r\n---\r\n> #include <aspect\/blackboard.h>\r\n> #include <aspect\/blocked_timing.h>\r\n29,33d29\r\n< #include <aspect\/blackboard.h>\r\n< #include <aspect\/blocked_timing.h>\r\n< #include <plugins\/gazebo\/aspect\/gazebo.h>\r\n< #include <string.h>\r\n< \r\n34a31\r\n> #include <core\/threading\/thread.h>\r\n36a34\r\n> #include <plugins\/gazebo\/aspect\/gazebo.h>\r\n37a36\r\n> #include <string.h>\r\n40d38\r\n< #include <gazebo\/transport\/TransportTypes.hh>\r\n41a40\r\n> #include <gazebo\/transport\/TransportTypes.hh>\r\n44,52c43,50\r\n< class DepthcamSimThread\r\n< : public fawkes::Thread,\r\n<   public fawkes::ClockAspect,\r\n<   public fawkes::LoggingAspect,\r\n<   public fawkes::ConfigurableAspect,\r\n<   public fawkes::BlackBoardAspect,\r\n<   public fawkes::BlockedTimingAspect,\r\n<   public fawkes::GazeboAspect,\r\n<   public fawkes::PointCloudAspect\r\n---\r\n> class DepthcamSimThread : public fawkes::Thread,\r\n>                           public fawkes::ClockAspect,\r\n>                           public fawkes::LoggingAspect,\r\n>                           public fawkes::ConfigurableAspect,\r\n>                           public fawkes::BlackBoardAspect,\r\n>                           public fawkes::BlockedTimingAspect,\r\n>                           public fawkes::GazeboAspect,\r\n>                           public fawkes::PointCloudAspect\r\n54,55c52,53\r\n<  public:\r\n<   DepthcamSimThread();\r\n---\r\n> public:\r\n> \tDepthcamSimThread();\r\n57,75c55,73\r\n<   virtual void init();\r\n<   virtual void loop();\r\n<   virtual void finalize();\r\n< \r\n<  private:\r\n<   gazebo::transport::SubscriberPtr depthcam_sub_;\r\n< \r\n<   \/\/handler function for incoming depthcam data messages\r\n<   void on_depthcam_data_msg(ConstPointCloudPtr &msg);\r\n< \r\n<   \/\/config values\r\n<   \/\/topic name of the gazebo message publisher\r\n<   std::string topic_name_;\r\n<   unsigned int width_;\r\n<   unsigned int height_;\r\n<   \/\/id of the shared memory object\r\n<   std::string shm_id_;\r\n<   std::string frame_;\r\n<   std::string pcl_id_;\r\n---\r\n> \tvirtual void init();\r\n> \tvirtual void loop();\r\n> \tvirtual void finalize();\r\n> \r\n> private:\r\n> \tgazebo::transport::SubscriberPtr depthcam_sub_;\r\n> \r\n> \t\/\/handler function for incoming depthcam data messages\r\n> \tvoid on_depthcam_data_msg(ConstPointCloudPtr &msg);\r\n> \r\n> \t\/\/config values\r\n> \t\/\/topic name of the gazebo message publisher\r\n> \tstd::string  topic_name_;\r\n> \tunsigned int width_;\r\n> \tunsigned int height_;\r\n> \t\/\/id of the shared memory object\r\n> \tstd::string shm_id_;\r\n> \tstd::string frame_;\r\n> \tstd::string pcl_id_;\r\n77c75\r\n<   fawkes::RefPtr<pcl::PointCloud<pcl::PointXYZ> > pcl_;\r\n---\r\n> \tfawkes::RefPtr<pcl::PointCloud<pcl::PointXYZ>> pcl_;\r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/gazsim-depthcam\/Makefile fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/gazsim-depthcam\/Makefile\r\n26,28c26,28\r\n< LIBS_gazsim_depthcam = fawkescore fawkesutils fawkesaspects fvutils \\\r\n< \t       fawkestf fawkesinterface fawkesblackboard \\\r\n< \t       fawkesgazeboaspect\r\n---\r\n> LIBS_gazsim_depthcam = m fawkescore fawkesutils fawkesaspects fvutils \\\r\n>                        fawkestf fawkesinterface fawkesblackboard \\\r\n>                        fawkesgazeboaspect\r\n31a32\r\n> PLUGINS_all = $(PLUGINDIR)\/gazsim-depthcam.so\r\n37c38,39\r\n<   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) $(LDFLAGS_PCL) -lm $(call boost-libs-ldflags,system) -lboost_system\r\n---\r\n>   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) $(LDFLAGS_PCL) \\\r\n>              $(call boost-libs-ldflags,system)\r\n39c41\r\n<   PLUGINS_all = $(PLUGINDIR)\/gazsim-depthcam.so\r\n---\r\n>   PLUGINS_build = $(PLUGINS_all)\r\ndiff -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\r\n21,22d20\r\n< #include <core\/plugin.h>\r\n< \r\n24a23,24\r\n> #include <core\/plugin.h>\r\n> \r\n34,36c34,35\r\n<  public:\r\n< \r\n<   \/** Constructor.\r\n---\r\n> public:\r\n> \t\/** Constructor.\r\n39,43c38,41\r\n<   GazsimLaserPlugin(Configuration *config)\r\n<     : Plugin(config)\r\n<   {\r\n<     thread_list.push_back(new LaserSimThread());\r\n<   }\r\n---\r\n> \texplicit GazsimLaserPlugin(Configuration *config) : Plugin(config)\r\n> \t{\r\n> \t\tthread_list.push_back(new LaserSimThread());\r\n> \t}\r\ndiff -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\r\n24,25c24\r\n< #include <tf\/types.h>\r\n< #include <utils\/math\/angle.h>\r\n---\r\n> #include <aspect\/logging.h>\r\n27d25\r\n< \r\n28a27,28\r\n> #include <tf\/types.h>\r\n> #include <utils\/math\/angle.h>\r\n30c30,31\r\n< #include <gazebo\/transport\/Node.hh>\r\n---\r\n> #include <cmath>\r\n> #include <cstdio>\r\n31a33\r\n> #include <gazebo\/transport\/Node.hh>\r\n33,36d34\r\n< #include <aspect\/logging.h>\r\n< \r\n< #include <cstdio>\r\n< #include <cmath>\r\n48,49c46,47\r\n<   : Thread("LaserSimThread", Thread::OPMODE_WAITFORWAKEUP),\r\n<     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)\r\n---\r\n> : Thread("LaserSimThread", Thread::OPMODE_WAITFORWAKEUP),\r\n>   BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)\r\n53c51,52\r\n< void LaserSimThread::init()\r\n---\r\n> void\r\n> LaserSimThread::init()\r\n55c54\r\n<   logger->log_debug(name(), "Initializing Simulation of the Laser Sensor");\r\n---\r\n> \tlogger->log_debug(name(), "Initializing Simulation of the Laser Sensor");\r\n57,73c56,72\r\n<   \/\/read config values\r\n<   max_range_ = config->get_float("\/gazsim\/laser\/max_range");\r\n<   laser_topic_ = config->get_string("\/gazsim\/topics\/laser");\r\n<   interface_id_ = config->get_string("\/gazsim\/laser\/interface-id");\r\n<   frame_id_ = config->get_string("\/gazsim\/laser\/frame-id");\r\n< \r\n<   \/\/open interface\r\n<   laser_if_ = blackboard->open_for_writing<Laser360Interface>(interface_id_.c_str());\r\n<   laser_if_->set_auto_timestamping(false);\r\n< \r\n<   \/\/subscribing to gazebo publisher\r\n<   laser_sub_ = gazebonode->Subscribe(laser_topic_, &LaserSimThread::on_laser_data_msg, this);\r\n< \r\n<   \/\/initialize laser data\r\n<   laser_data_ = (float *)malloc(sizeof(float) * 360);\r\n<   laser_time_ = new Time(clock);\r\n<   new_data_ = false;\r\n---\r\n> \t\/\/read config values\r\n> \tmax_range_    = config->get_float("\/gazsim\/laser\/max_range");\r\n> \tlaser_topic_  = config->get_string("\/gazsim\/topics\/laser");\r\n> \tinterface_id_ = config->get_string("\/gazsim\/laser\/interface-id");\r\n> \tframe_id_     = config->get_string("\/gazsim\/laser\/frame-id");\r\n> \r\n> \t\/\/open interface\r\n> \tlaser_if_ = blackboard->open_for_writing<Laser360Interface>(interface_id_.c_str());\r\n> \tlaser_if_->set_auto_timestamping(false);\r\n> \r\n> \t\/\/subscribing to gazebo publisher\r\n> \tlaser_sub_ = gazebonode->Subscribe(laser_topic_, &LaserSimThread::on_laser_data_msg, this);\r\n> \r\n> \t\/\/initialize laser data\r\n> \tlaser_data_ = (float *)malloc(sizeof(float) * 360);\r\n> \tlaser_time_ = new Time(clock);\r\n> \tnew_data_   = false;\r\n75,76c74,75\r\n<   \/\/set frame in the interface\r\n<   laser_if_->set_frame(frame_id_.c_str());\r\n---\r\n> \t\/\/set frame in the interface\r\n> \tlaser_if_->set_frame(frame_id_.c_str());\r\n79c78,79\r\n< void LaserSimThread::finalize()\r\n---\r\n> void\r\n> LaserSimThread::finalize()\r\n81,83c81,83\r\n<   blackboard->close(laser_if_);\r\n<   free(laser_data_);\r\n<   delete laser_time_;\r\n---\r\n> \tblackboard->close(laser_if_);\r\n> \tfree(laser_data_);\r\n> \tdelete laser_time_;\r\n86c86,87\r\n< void LaserSimThread::loop()\r\n---\r\n> void\r\n> LaserSimThread::loop()\r\n88,93c89,93\r\n<   if(new_data_)\r\n<   {\r\n<     \/\/write interface\r\n<     laser_if_->set_distances(laser_data_);\r\n<     laser_if_->set_timestamp(laser_time_);\r\n<     laser_if_->write();\r\n---\r\n> \tif (new_data_) {\r\n> \t\t\/\/write interface\r\n> \t\tlaser_if_->set_distances(laser_data_);\r\n> \t\tlaser_if_->set_timestamp(laser_time_);\r\n> \t\tlaser_if_->write();\r\n95,96c95,96\r\n<     new_data_ = false;\r\n<   }  \r\n---\r\n> \t\tnew_data_ = false;\r\n> \t}\r\n99c99,100\r\n< void LaserSimThread::on_laser_data_msg(ConstLaserScanStampedPtr &msg)\r\n---\r\n> void\r\n> LaserSimThread::on_laser_data_msg(ConstLaserScanStampedPtr &msg)\r\n101c102,106\r\n<   \/\/logger->log_info(name(), "Got new Laser data.\\n");\r\n---\r\n> \t\/\/logger->log_info(name(), "Got new Laser data.\\n");\r\n> \r\n> \tMutexLocker lock(loop_mutex);\r\n> \r\n> \tconst gazebo::msgs::LaserScan &scan = msg->scan();\r\n103c108,109\r\n<   MutexLocker lock(loop_mutex);\r\n---\r\n> \t\/\/calculate start angle\r\n> \tint start_index = (scan.angle_min() + 2 * M_PI) \/ M_PI * 180;\r\n105c111\r\n<   const gazebo::msgs::LaserScan &scan = msg->scan();\r\n---\r\n> \tint number_beams = scan.ranges_size();\r\n107,125c113\r\n<   \/\/calculate start angle\r\n<   int start_index = (scan.angle_min() + 2* M_PI) \/ M_PI * 180;\r\n<   \r\n<   int number_beams = scan.ranges_size();\r\n< \r\n<   *laser_time_ = clock->now();\r\n<   \r\n<   \/\/copy laser data\r\n<   for(int i = 0; i < number_beams; i++)\r\n<   {\r\n<     const float range = scan.ranges(i);\r\n<     if(range < max_range_)\r\n<     {\r\n<       laser_data_[(start_index + i) % 360] = range; \r\n<     }\r\n<     else\r\n<     {\r\n<       laser_data_[(start_index + i) % 360] = NAN;\r\n<     }\r\n---\r\n> \t*laser_time_ = clock->now();\r\n127,128c115,124\r\n<   }\r\n<   new_data_  = true;\r\n---\r\n> \t\/\/copy laser data\r\n> \tfor (int i = 0; i < number_beams; i++) {\r\n> \t\tconst float range = scan.ranges(i);\r\n> \t\tif (range < max_range_) {\r\n> \t\t\tlaser_data_[(start_index + i) % 360] = range;\r\n> \t\t} else {\r\n> \t\t\tlaser_data_[(start_index + i) % 360] = NAN;\r\n> \t\t}\r\n> \t}\r\n> \tnew_data_ = true;\r\ndiff -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\r\n21,22c21,22\r\n< #ifndef __PLUGINS_GAZSIM_LASER_THREAD_H_\r\n< #define __PLUGINS_GAZSIM_LASER_THREAD_H_\r\n---\r\n> #ifndef _PLUGINS_GAZSIM_LASER_THREAD_H_\r\n> #define _PLUGINS_GAZSIM_LASER_THREAD_H_\r\n24c24,25\r\n< #include <core\/threading\/thread.h>\r\n---\r\n> #include <aspect\/blackboard.h>\r\n> #include <aspect\/blocked_timing.h>\r\n28,29c29\r\n< #include <aspect\/blackboard.h>\r\n< #include <aspect\/blocked_timing.h>\r\n---\r\n> #include <core\/threading\/thread.h>\r\n33d32\r\n< #include <gazebo\/transport\/TransportTypes.hh>\r\n34a34\r\n> #include <gazebo\/transport\/TransportTypes.hh>\r\n37d36\r\n< \r\n39,50c38,48\r\n<   class Laser360Interface;\r\n<   class Time;\r\n< }\r\n< \r\n< class LaserSimThread\r\n< : public fawkes::Thread,\r\n<   public fawkes::ClockAspect,\r\n<   public fawkes::LoggingAspect,\r\n<   public fawkes::ConfigurableAspect,\r\n<   public fawkes::BlackBoardAspect,\r\n<   public fawkes::BlockedTimingAspect,\r\n<   public fawkes::GazeboAspect\r\n---\r\n> class Laser360Interface;\r\n> class Time;\r\n> } \/\/ namespace fawkes\r\n> \r\n> class LaserSimThread : public fawkes::Thread,\r\n>                        public fawkes::ClockAspect,\r\n>                        public fawkes::LoggingAspect,\r\n>                        public fawkes::ConfigurableAspect,\r\n>                        public fawkes::BlackBoardAspect,\r\n>                        public fawkes::BlockedTimingAspect,\r\n>                        public fawkes::GazeboAspect\r\n52,53c50,51\r\n<  public:\r\n<   LaserSimThread();\r\n---\r\n> public:\r\n> \tLaserSimThread();\r\n55,57c53,55\r\n<   virtual void init();\r\n<   virtual void loop();\r\n<   virtual void finalize();\r\n---\r\n> \tvirtual void init();\r\n> \tvirtual void loop();\r\n> \tvirtual void finalize();\r\n59,62c57,60\r\n<  private:\r\n<   \/\/\/Subscriber to receive laser data from gazebo\r\n<   gazebo::transport::SubscriberPtr laser_sub_;\r\n<   std::string laser_topic_;\r\n---\r\n> private:\r\n> \t\/\/\/Subscriber to receive laser data from gazebo\r\n> \tgazebo::transport::SubscriberPtr laser_sub_;\r\n> \tstd::string                      laser_topic_;\r\n64,65c62,63\r\n<   \/\/\/provided interface\r\n<   fawkes::Laser360Interface *laser_if_;\r\n---\r\n> \t\/\/\/provided interface\r\n> \tfawkes::Laser360Interface *laser_if_;\r\n67,69c65,67\r\n<   \/\/\/storage for laser data\r\n<   float        *laser_data_;\r\n<   fawkes::Time *laser_time_;\r\n---\r\n> \t\/\/\/storage for laser data\r\n> \tfloat *       laser_data_;\r\n> \tfawkes::Time *laser_time_;\r\n71,72c69,70\r\n<   \/\/\/is there new information to write in the interface?\r\n<   bool new_data_;\r\n---\r\n> \t\/\/\/is there new information to write in the interface?\r\n> \tbool new_data_;\r\n74,75c72,73\r\n<   \/\/\/handler function for incoming laser data messages\r\n<   void on_laser_data_msg(ConstLaserScanStampedPtr &msg);\r\n---\r\n> \t\/\/\/handler function for incoming laser data messages\r\n> \tvoid on_laser_data_msg(ConstLaserScanStampedPtr &msg);\r\n77,78c75,76\r\n<   \/\/\/maximal range of the laser sensor\r\n<   float max_range_;  \r\n---\r\n> \t\/\/\/maximal range of the laser sensor\r\n> \tfloat max_range_;\r\n80,81c78,79\r\n<   std::string interface_id_;\r\n<   std::string frame_id_;\r\n---\r\n> \tstd::string interface_id_;\r\n> \tstd::string frame_id_;\r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/gazsim-laser\/Makefile fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/gazsim-laser\/Makefile\r\n31a32\r\n> PLUGINS_all = $(PLUGINDIR)\/gazsim-laser.so\r\n39c40\r\n<   PLUGINS_all = $(PLUGINDIR)\/gazsim-laser.so\r\n---\r\n>   PLUGINS_build = $(PLUGINS_all)\r\ndiff -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\r\n21,22d20\r\n< #include <math.h>\r\n< \r\n24a23,26\r\n> #include <utils\/misc\/gazebo_api_wrappers.h>\r\n> \r\n> #include <math.h>\r\n> \r\n37c39\r\n<   printf("Destructing Gps Plugin!\\n");\r\n---\r\n> \tprintf("Destructing Gps Plugin!\\n");\r\n43c45,46\r\n< void Gps::Load(physics::ModelPtr _parent, sdf::ElementPtr \/*_sdf*\/) \r\n---\r\n> void\r\n> Gps::Load(physics::ModelPtr _parent, sdf::ElementPtr \/*_sdf*\/)\r\n45,46c48,49\r\n<   \/\/ Store the pointer to the model\r\n<   this->model_ = _parent;\r\n---\r\n> \t\/\/ Store the pointer to the model\r\n> \tthis->model_ = _parent;\r\n48,50c51,65\r\n<   \/\/get the model-name\r\n<   this->name_ = model_->GetName();\r\n<   printf("Loading Gps Plugin of model %s\\n", name_.c_str());\r\n---\r\n> \t\/\/get the model-name\r\n> \tthis->name_ = model_->GetName();\r\n> \tprintf("Loading Gps Plugin of model %s\\n", name_.c_str());\r\n> \r\n> \t\/\/ Listen to the update event. This event is broadcast every\r\n> \t\/\/ simulation iteration.\r\n> \tthis->update_connection_ =\r\n> \t  event::Events::ConnectWorldUpdateBegin(boost::bind(&Gps::OnUpdate, this, _1));\r\n> \r\n> \t\/\/Create the communication Node for communication with fawkes\r\n> \tthis->node_ = transport::NodePtr(new transport::Node());\r\n> \r\n> \t\/\/set namespace to the model name & init last sent time\r\n> \tthis->node_->Init(model_->GetWorld()->GZWRAP_NAME() + "\/" + name_);\r\n> \tlast_sent_time_ = model_->GetWorld()->GZWRAP_SIM_TIME().Double();\r\n52,65c67,68\r\n<   \/\/ Listen to the update event. This event is broadcast every\r\n<   \/\/ simulation iteration.\r\n<   this->update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&Gps::OnUpdate, this, _1));\r\n< \r\n<   \/\/Create the communication Node for communication with fawkes\r\n<   this->node_ = transport::NodePtr(new transport::Node());\r\n<   \/\/the namespace is set to the model name!\r\n<   this->node_->Init(model_->GetWorld()->GetName()+"\/"+name_);\r\n< \r\n<   \/\/init last sent time\r\n<   last_sent_time_ = model_->GetWorld()->GetSimTime().Double();\r\n< \r\n<   \/\/create publisher\r\n<   this->gps_pub_ = this->node_->Advertise<msgs::Pose>("~\/gazsim\/gps\/");\r\n---\r\n> \t\/\/create publisher\r\n> \tthis->gps_pub_ = this->node_->Advertise<msgs::Pose>("~\/gazsim\/gps\/");\r\n68d70\r\n< \r\n71c73,74\r\n< void Gps::OnUpdate(const common::UpdateInfo & \/*_info*\/)\r\n---\r\n> void\r\n> Gps::OnUpdate(const common::UpdateInfo & \/*_info*\/)\r\n73,79c76,82\r\n<   \/\/Send position information to Fawkes\r\n<   double time = model_->GetWorld()->GetSimTime().Double();\r\n<   if(time - last_sent_time_ > (1.0 \/ 10.0))\r\n<   {\r\n<     last_sent_time_ = time;\r\n<     send_position();\r\n<   }\r\n---\r\n> \t\/\/Send position information to Fawkes\r\n> \tdouble time = model_->GetWorld()->GZWRAP_SIM_TIME().Double();\r\n> \r\n> \tif (time - last_sent_time_ > (1.0 \/ 10.0)) {\r\n> \t\tlast_sent_time_ = time;\r\n> \t\tsend_position();\r\n> \t}\r\n84c87,88\r\n< void Gps::Reset()\r\n---\r\n> void\r\n> Gps::Reset()\r\n91c95,96\r\n< void Gps::send_position()\r\n---\r\n> void\r\n> Gps::send_position()\r\n93,107c98,111\r\n<   if(gps_pub_->HasConnections())\r\n<   {\r\n<     \/\/build message\r\n<     msgs::Pose posMsg;\r\n<     posMsg.mutable_position()->set_x(this->model_->GetWorldPose().pos.x);\r\n<     posMsg.mutable_position()->set_y(this->model_->GetWorldPose().pos.y);\r\n<     posMsg.mutable_position()->set_z(this->model_->GetWorldPose().pos.z);\r\n<     posMsg.mutable_orientation()->set_x(this->model_->GetWorldPose().rot.x);\r\n<     posMsg.mutable_orientation()->set_y(this->model_->GetWorldPose().rot.y);\r\n<     posMsg.mutable_orientation()->set_z(this->model_->GetWorldPose().rot.z);\r\n<     posMsg.mutable_orientation()->set_w(this->model_->GetWorldPose().rot.w);\r\n< \r\n<     \/\/send\r\n<     gps_pub_->Publish(posMsg);\r\n<   }\r\n---\r\n> \tif (gps_pub_->HasConnections()) {\r\n> \t\t\/\/build message\r\n> \t\tmsgs::Pose posMsg;\r\n> \t\tposMsg.mutable_position()->set_x(this->model_->GZWRAP_WORLD_POSE().GZWRAP_POS_X);\r\n> \t\tposMsg.mutable_position()->set_y(this->model_->GZWRAP_WORLD_POSE().GZWRAP_POS_Y);\r\n> \t\tposMsg.mutable_position()->set_z(this->model_->GZWRAP_WORLD_POSE().GZWRAP_POS_Z);\r\n> \t\tposMsg.mutable_orientation()->set_x(this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_X);\r\n> \t\tposMsg.mutable_orientation()->set_y(this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_Y);\r\n> \t\tposMsg.mutable_orientation()->set_z(this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_Z);\r\n> \t\tposMsg.mutable_orientation()->set_w(this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_W);\r\n> \r\n> \t\t\/\/send\r\n> \t\tgps_pub_->Publish(posMsg);\r\n> \t}\r\ndiff -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\r\n21a22\r\n> #include <gazebo\/common\/common.hh>\r\n24,25d24\r\n< #include <gazebo\/common\/common.hh>\r\n< #include <stdio.h>\r\n27a27\r\n> #include <stdio.h>\r\n30,32c30,31\r\n< namespace gazebo\r\n< {\r\n<   \/**\r\n---\r\n> namespace gazebo {\r\n> \/**\r\n36,67c35,66\r\n<   class Gps : public ModelPlugin\r\n<   {\r\n<   public:\r\n<     Gps();\r\n<    ~Gps();\r\n< \r\n<     \/\/Overridden ModelPlugin-Functions\r\n<     virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr \/*_sdf*\/);\r\n<     virtual void OnUpdate(const common::UpdateInfo &);\r\n<     virtual void Reset();\r\n< \r\n<   private:\r\n<     \/\/\/ Pointer to the gazbeo model\r\n<     physics::ModelPtr model_;\r\n<     \/\/\/ Pointer to the update event connection\r\n<     event::ConnectionPtr update_connection_;\r\n<     \/\/\/Node for communication to fawkes\r\n<     transport::NodePtr node_;\r\n<     \/\/\/name of the gps and the communication channel\r\n<     std::string name_;\r\n< \r\n<     \/\/\/time variable to send in intervals\r\n<     double last_sent_time_;\r\n< \r\n<     \/\/Gps Stuff:\r\n<     \/\/\/Functions for sending information to fawkes:\r\n<     void send_position();\r\n< \r\n<     \/\/\/Publisher for GyroAngle\r\n<     transport::PublisherPtr gps_pub_;\r\n<   };\r\n< }\r\n---\r\n> class Gps : public ModelPlugin\r\n> {\r\n> public:\r\n> \tGps();\r\n> \t~Gps();\r\n> \r\n> \t\/\/Overridden ModelPlugin-Functions\r\n> \tvirtual void Load(physics::ModelPtr _parent, sdf::ElementPtr \/*_sdf*\/);\r\n> \tvirtual void OnUpdate(const common::UpdateInfo &);\r\n> \tvirtual void Reset();\r\n> \r\n> private:\r\n> \t\/\/\/ Pointer to the gazbeo model\r\n> \tphysics::ModelPtr model_;\r\n> \t\/\/\/ Pointer to the update event connection\r\n> \tevent::ConnectionPtr update_connection_;\r\n> \t\/\/\/Node for communication to fawkes\r\n> \ttransport::NodePtr node_;\r\n> \t\/\/\/name of the gps and the communication channel\r\n> \tstd::string name_;\r\n> \r\n> \t\/\/\/time variable to send in intervals\r\n> \tdouble last_sent_time_;\r\n> \r\n> \t\/\/Gps Stuff:\r\n> \t\/\/\/Functions for sending information to fawkes:\r\n> \tvoid send_position();\r\n> \r\n> \t\/\/\/Publisher for GyroAngle\r\n> \ttransport::PublisherPtr gps_pub_;\r\n> };\r\n> } \/\/ namespace gazebo\r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/gazsim-localization\/gazebo-plugin\/Makefile fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/gazsim-localization\/gazebo-plugin\/Makefile\r\n26c26\r\n< LIBS_gazebo_libgps = gazsim_msgs\r\n---\r\n> LIBS_gazebo_libgps = m gazsim_msgs\r\n32c32,33\r\n< OBJS_all    = $(OBJS_gazebo_libgps)\r\n---\r\n> OBJS_all = $(OBJS_gazebo_libgps)\r\n> LIBS_all = $(LIBDIR)\/gazebo\/libgps.so\r\n36c37,38\r\n<   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) -lm $(call boost-libs-ldflags,system) -lboost_system\r\n---\r\n>   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) \\\r\n>              $(call boost-libs-ldflags,system)\r\n38c40\r\n<   LIBS_all = $(LIBDIR)\/gazebo\/libgps.so\r\n---\r\n>   LIBS_build = $(LIBS_all)\r\ndiff -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\r\n22,23d21\r\n< #include <core\/plugin.h>\r\n< \r\n25a24,25\r\n> #include <core\/plugin.h>\r\n> \r\n35,37c35,36\r\n<  public:\r\n< \r\n<   \/** Constructor.\r\n---\r\n> public:\r\n> \t\/** Constructor.\r\n40,44c39,42\r\n<   GazsimLocalizationPlugin(Configuration *config)\r\n<     : Plugin(config)\r\n<   {\r\n<     thread_list.push_back(new LocalizationSimThread());\r\n<   }\r\n---\r\n> \texplicit GazsimLocalizationPlugin(Configuration *config) : Plugin(config)\r\n> \t{\r\n> \t\tthread_list.push_back(new LocalizationSimThread());\r\n> \t}\r\ndiff -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\r\n24,27c24\r\n< #include <tf\/types.h>\r\n< #include <stdio.h>\r\n< #include <math.h>\r\n< #include <utils\/math\/angle.h>\r\n---\r\n> #include <aspect\/logging.h>\r\n29d25\r\n< \r\n30a27,30\r\n> #include <tf\/transform_publisher.h>\r\n> #include <tf\/types.h>\r\n> #include <utils\/math\/angle.h>\r\n> #include <utils\/time\/clock.h>\r\n32d31\r\n< #include <gazebo\/transport\/Node.hh>\r\n33a33\r\n> #include <gazebo\/transport\/Node.hh>\r\n35,37c35,36\r\n< #include <aspect\/logging.h>\r\n< #include <tf\/transform_publisher.h>\r\n< #include <utils\/time\/clock.h>\r\n---\r\n> #include <math.h>\r\n> #include <stdio.h>\r\n49,51c48,50\r\n<   : Thread("LocalizationSimThread", Thread::OPMODE_WAITFORWAKEUP),\r\n<     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS),\r\n<     TransformAspect(TransformAspect::BOTH, "Pose")\r\n---\r\n> : Thread("LocalizationSimThread", Thread::OPMODE_WAITFORWAKEUP),\r\n>   BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS),\r\n>   TransformAspect(TransformAspect::BOTH, "Pose")\r\n55c54,55\r\n< void LocalizationSimThread::init()\r\n---\r\n> void\r\n> LocalizationSimThread::init()\r\n57,60c57\r\n<   logger->log_debug(name(), "Initializing Simulation of the Localization");\r\n< \r\n<   \/\/open interface\r\n<   localization_if_ = blackboard->open_for_writing<Position3DInterface>("Pose");\r\n---\r\n> \tlogger->log_debug(name(), "Initializing Simulation of the Localization");\r\n62,66c59,60\r\n<   \/\/read cofig values\r\n<   gps_topic_ = config->get_string("\/gazsim\/topics\/gps");\r\n<   transform_tolerance_ = config->get_float("\/plugins\/amcl\/transform_tolerance");\r\n<   global_frame_id_ = config->get_string("\/plugins\/amcl\/global_frame_id");\r\n<   odom_frame_id_ = config->get_string("\/plugins\/amcl\/odom_frame_id");\r\n---\r\n> \t\/\/open interface\r\n> \tlocalization_if_ = blackboard->open_for_writing<Position3DInterface>("Pose");\r\n68,69c62,70\r\n<   \/\/subscribing to gazebo publisher\r\n<   localization_sub_ = gazebonode->Subscribe(gps_topic_, &LocalizationSimThread::on_localization_msg, this);\r\n---\r\n> \t\/\/read cofig values\r\n> \tgps_topic_           = config->get_string("\/gazsim\/topics\/gps");\r\n> \ttransform_tolerance_ = config->get_float("\/plugins\/amcl\/transform_tolerance");\r\n> \tglobal_frame_id_     = config->get_string("\/plugins\/amcl\/global_frame_id");\r\n> \todom_frame_id_       = config->get_string("\/plugins\/amcl\/odom_frame_id");\r\n> \r\n> \t\/\/subscribing to gazebo publisher\r\n> \tlocalization_sub_ =\r\n> \t  gazebonode->Subscribe(gps_topic_, &LocalizationSimThread::on_localization_msg, this);\r\n71c72\r\n<   new_data_ = false;\r\n---\r\n> \tnew_data_ = false;\r\n74c75,76\r\n< void LocalizationSimThread::finalize()\r\n---\r\n> void\r\n> LocalizationSimThread::finalize()\r\n76c78\r\n<   blackboard->close(localization_if_);\r\n---\r\n> \tblackboard->close(localization_if_);\r\n79c81,82\r\n< void LocalizationSimThread::loop()\r\n---\r\n> void\r\n> LocalizationSimThread::loop()\r\n81,102c84,105\r\n<   if(new_data_)\r\n<   {\r\n<     \/\/write interface\r\n<     localization_if_->set_frame(global_frame_id_.c_str());\r\n<     localization_if_->set_visibility_history(100);\r\n<     localization_if_->set_translation(0, x_);\r\n<     localization_if_->set_translation(1, y_);\r\n<     localization_if_->set_translation(2, z_);\r\n<     localization_if_->set_rotation(0, quat_x_);\r\n<     localization_if_->set_rotation(1, quat_y_);\r\n<     localization_if_->set_rotation(2, quat_z_);\r\n<     localization_if_->set_rotation(3, quat_w_);\r\n<     localization_if_->write();\r\n< \r\n<     \/\/publish transform (similar as in amcl_thread.cpp)\r\n<     tf::Quaternion rotation = tf::Quaternion(quat_x_, quat_y_, quat_z_, quat_w_);\r\n<     tf::Point position = tf::Point(x_, y_, z_);\r\n<     tf::Transform latest_tf_ = tf::Transform(rotation,position);\r\n<     Time transform_expiration =\r\n<       (clock->now() + transform_tolerance_);\r\n<     tf_publisher->send_transform(latest_tf_, transform_expiration,\r\n< \t\t\t\t global_frame_id_, odom_frame_id_);\r\n---\r\n> \tif (new_data_) {\r\n> \t\t\/\/write interface\r\n> \t\tlocalization_if_->set_frame(global_frame_id_.c_str());\r\n> \t\tlocalization_if_->set_visibility_history(100);\r\n> \t\tlocalization_if_->set_translation(0, x_);\r\n> \t\tlocalization_if_->set_translation(1, y_);\r\n> \t\tlocalization_if_->set_translation(2, z_);\r\n> \t\tlocalization_if_->set_rotation(0, quat_x_);\r\n> \t\tlocalization_if_->set_rotation(1, quat_y_);\r\n> \t\tlocalization_if_->set_rotation(2, quat_z_);\r\n> \t\tlocalization_if_->set_rotation(3, quat_w_);\r\n> \t\tlocalization_if_->write();\r\n> \r\n> \t\t\/\/publish transform (similar as in amcl_thread.cpp)\r\n> \t\ttf::Quaternion rotation             = tf::Quaternion(quat_x_, quat_y_, quat_z_, quat_w_);\r\n> \t\ttf::Point      position             = tf::Point(x_, y_, z_);\r\n> \t\ttf::Transform  latest_tf_           = tf::Transform(rotation, position);\r\n> \t\tTime           transform_expiration = (clock->now() + transform_tolerance_);\r\n> \t\ttf_publisher->send_transform(latest_tf_,\r\n> \t\t                             transform_expiration,\r\n> \t\t                             global_frame_id_,\r\n> \t\t                             odom_frame_id_);\r\n104,105c107,108\r\n<     new_data_ = false;\r\n<   }\r\n---\r\n> \t\tnew_data_ = false;\r\n> \t}\r\n108c111,112\r\n< void LocalizationSimThread::on_localization_msg(ConstPosePtr &msg)\r\n---\r\n> void\r\n> LocalizationSimThread::on_localization_msg(ConstPosePtr &msg)\r\n110,123c114,127\r\n<   \/\/logger->log_info(name(), "Got new Localization data.\\n");\r\n<   \r\n<   MutexLocker lock(loop_mutex);\r\n< \r\n<   \/\/read data from message\r\n<   x_ = msg->position().x();\r\n<   y_ = msg->position().y();\r\n<   z_ = msg->position().z();\r\n<   quat_x_ = msg->orientation().x();\r\n<   quat_y_ = msg->orientation().y();\r\n<   quat_z_ = msg->orientation().z();\r\n<   quat_w_ = msg->orientation().w();\r\n<   \r\n<   new_data_ = true;\r\n---\r\n> \t\/\/logger->log_info(name(), "Got new Localization data.\\n");\r\n> \r\n> \tMutexLocker lock(loop_mutex);\r\n> \r\n> \t\/\/read data from message\r\n> \tx_      = msg->position().x();\r\n> \ty_      = msg->position().y();\r\n> \tz_      = msg->position().z();\r\n> \tquat_x_ = msg->orientation().x();\r\n> \tquat_y_ = msg->orientation().y();\r\n> \tquat_z_ = msg->orientation().z();\r\n> \tquat_w_ = msg->orientation().w();\r\n> \r\n> \tnew_data_ = true;\r\ndiff -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\r\n22,23c22,23\r\n< #ifndef __PLUGINS_GAZSIM_LOCALIZATION_THREAD_H_\r\n< #define __PLUGINS_GAZSIM_LOCALIZATION_THREAD_H_\r\n---\r\n> #ifndef _PLUGINS_GAZSIM_LOCALIZATION_THREAD_H_\r\n> #define _PLUGINS_GAZSIM_LOCALIZATION_THREAD_H_\r\n25c25,26\r\n< #include <core\/threading\/thread.h>\r\n---\r\n> #include <aspect\/blackboard.h>\r\n> #include <aspect\/blocked_timing.h>\r\n29,30d29\r\n< #include <aspect\/blackboard.h>\r\n< #include <aspect\/blocked_timing.h>\r\n31a31\r\n> #include <core\/threading\/thread.h>\r\n35d34\r\n< #include <gazebo\/transport\/TransportTypes.hh>\r\n36a36\r\n> #include <gazebo\/transport\/TransportTypes.hh>\r\n39d38\r\n< \r\n41c40\r\n<   class Position3DInterface;\r\n---\r\n> class Position3DInterface;\r\n44,52c43,50\r\n< class LocalizationSimThread\r\n< : public fawkes::Thread,\r\n<   public fawkes::ClockAspect,\r\n<   public fawkes::LoggingAspect,\r\n<   public fawkes::ConfigurableAspect,\r\n<   public fawkes::BlackBoardAspect,\r\n<   public fawkes::BlockedTimingAspect,\r\n<   public fawkes::GazeboAspect,\r\n<   public fawkes::TransformAspect\r\n---\r\n> class LocalizationSimThread : public fawkes::Thread,\r\n>                               public fawkes::ClockAspect,\r\n>                               public fawkes::LoggingAspect,\r\n>                               public fawkes::ConfigurableAspect,\r\n>                               public fawkes::BlackBoardAspect,\r\n>                               public fawkes::BlockedTimingAspect,\r\n>                               public fawkes::GazeboAspect,\r\n>                               public fawkes::TransformAspect\r\n54,55c52,53\r\n<  public:\r\n<   LocalizationSimThread();\r\n---\r\n> public:\r\n> \tLocalizationSimThread();\r\n57,89c55,87\r\n<   virtual void init();\r\n<   virtual void loop();\r\n<   virtual void finalize();\r\n< \r\n<  private:\r\n<   \/\/Subscriber to receive localization data from gazebo\r\n<   gazebo::transport::SubscriberPtr localization_sub_;\r\n<   std::string gps_topic_;\r\n< \r\n<   \/\/provided interface\r\n<   fawkes::Position3DInterface *localization_if_;\r\n< \r\n<   \/\/handler function for incoming localization data messages\r\n<   void on_localization_msg(ConstPosePtr &msg);\r\n< \r\n<   \/\/is there new information to write in the interface?\r\n<   bool new_data_;\r\n< \r\n<   \/\/localization data\r\n<   double x_;\r\n<   double y_;\r\n<   double z_;\r\n<   double quat_x_;\r\n<   double quat_y_;\r\n<   double quat_z_;\r\n<   double quat_w_;\r\n< \r\n<   \/\/time the transform should be up to date\r\n<   double transform_tolerance_;\r\n< \r\n<   \/\/frame ids for transform\r\n<   std::string odom_frame_id_;\r\n<   std::string global_frame_id_;\r\n---\r\n> \tvirtual void init();\r\n> \tvirtual void loop();\r\n> \tvirtual void finalize();\r\n> \r\n> private:\r\n> \t\/\/Subscriber to receive localization data from gazebo\r\n> \tgazebo::transport::SubscriberPtr localization_sub_;\r\n> \tstd::string                      gps_topic_;\r\n> \r\n> \t\/\/provided interface\r\n> \tfawkes::Position3DInterface *localization_if_;\r\n> \r\n> \t\/\/handler function for incoming localization data messages\r\n> \tvoid on_localization_msg(ConstPosePtr &msg);\r\n> \r\n> \t\/\/is there new information to write in the interface?\r\n> \tbool new_data_;\r\n> \r\n> \t\/\/localization data\r\n> \tdouble x_;\r\n> \tdouble y_;\r\n> \tdouble z_;\r\n> \tdouble quat_x_;\r\n> \tdouble quat_y_;\r\n> \tdouble quat_z_;\r\n> \tdouble quat_w_;\r\n> \r\n> \t\/\/time the transform should be up to date\r\n> \tdouble transform_tolerance_;\r\n> \r\n> \t\/\/frame ids for transform\r\n> \tstd::string odom_frame_id_;\r\n> \tstd::string global_frame_id_;\r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/gazsim-localization\/Makefile fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/gazsim-localization\/Makefile\r\n33a34\r\n> PLUGINS_all = $(PLUGINDIR)\/gazsim-localization.so\r\n41c42\r\n<   PLUGINS_all = $(PLUGINDIR)\/gazsim-localization.so\r\n---\r\n>   PLUGINS_build = $(PLUGINS_all)\r\ndiff -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\r\n21,22d20\r\n< #include <math.h>\r\n< \r\n24a23,26\r\n> #include <utils\/misc\/gazebo_api_wrappers.h>\r\n> \r\n> #include <math.h>\r\n> \r\n30c32\r\n< Gyro::Gyro()\r\n---\r\n> Gyro::Gyro() : last_sent_time_(0.0), send_interval_(0.0)\r\n36c38\r\n<   printf("Destructing Gyro Plugin!\\n");\r\n---\r\n> \tprintf("Destructing Gyro Plugin!\\n");\r\n42c44,45\r\n< void Gyro::Load(physics::ModelPtr _parent, sdf::ElementPtr \/*_sdf*\/) \r\n---\r\n> void\r\n> Gyro::Load(physics::ModelPtr _parent, sdf::ElementPtr \/*_sdf*\/)\r\n44,62c47,48\r\n<   \/\/ Store the pointer to the model\r\n<   this->model_ = _parent;\r\n< \r\n<   \/\/get the model-name\r\n<   this->name_ = model_->GetName();\r\n<   printf("Loading Gyro Plugin of model %s\\n", name_.c_str());\r\n< \r\n<   \/\/ Listen to the update event. This event is broadcast every\r\n<   \/\/ simulation iteration.\r\n<   this->update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&Gyro::OnUpdate, this, _1));\r\n< \r\n<   \/\/Create the communication Node for communication with fawkes\r\n<   this->node_ = transport::NodePtr(new transport::Node());\r\n<   \/\/the namespace is set to the model name!\r\n<   this->node_->Init(model_->GetWorld()->GetName()+"\/"+name_);\r\n< \r\n< \r\n<   \/\/create publisher\r\n<   this->gyro_pub_ = this->node_->Advertise<msgs::Vector3d>("~\/RobotinoSim\/Gyro\/");\r\n---\r\n> \t\/\/ Store the pointer to the model\r\n> \tthis->model_ = _parent;\r\n64,66c50,69\r\n<   \/\/init last sent time\r\n<   last_sent_time_ = model_->GetWorld()->GetSimTime().Double();\r\n<   this->send_interval_ = 0.05;\r\n---\r\n> \t\/\/get the model-name\r\n> \tthis->name_ = model_->GetName();\r\n> \tprintf("Loading Gyro Plugin of model %s\\n", name_.c_str());\r\n> \r\n> \t\/\/ Listen to the update event. This event is broadcast every\r\n> \t\/\/ simulation iteration.\r\n> \tthis->update_connection_ =\r\n> \t  event::Events::ConnectWorldUpdateBegin(boost::bind(&Gyro::OnUpdate, this, _1));\r\n> \r\n> \t\/\/Create the communication Node for communication with fawkes\r\n> \tthis->node_ = transport::NodePtr(new transport::Node());\r\n> \t\/\/the namespace is set to the model name!\r\n> \tthis->node_->Init(model_->GetWorld()->GZWRAP_NAME() + "\/" + name_);\r\n> \r\n> \t\/\/create publisher\r\n> \tthis->gyro_pub_ = this->node_->Advertise<msgs::Vector3d>("~\/RobotinoSim\/Gyro\/");\r\n> \r\n> \t\/\/init last sent time\r\n> \tlast_sent_time_      = model_->GetWorld()->GZWRAP_SIM_TIME().Double();\r\n> \tthis->send_interval_ = 0.05;\r\n71c74,75\r\n< void Gyro::OnUpdate(const common::UpdateInfo & \/*_info*\/)\r\n---\r\n> void\r\n> Gyro::OnUpdate(const common::UpdateInfo & \/*_info*\/)\r\n73,79c77,82\r\n<   \/\/Send gyro information to Fawkes\r\n<   double time = model_->GetWorld()->GetSimTime().Double();\r\n<   if(time - last_sent_time_ > send_interval_)\r\n<   {\r\n<     last_sent_time_ = time;\r\n<     send_gyro();\r\n<   }\r\n---\r\n> \t\/\/Send gyro information to Fawkes\r\n> \tdouble time = model_->GetWorld()->GZWRAP_SIM_TIME().Double();\r\n> \tif (time - last_sent_time_ > send_interval_) {\r\n> \t\tlast_sent_time_ = time;\r\n> \t\tsend_gyro();\r\n> \t}\r\n84c87,88\r\n< void Gyro::Reset()\r\n---\r\n> void\r\n> Gyro::Reset()\r\n88c92,93\r\n< void Gyro::send_gyro()\r\n---\r\n> void\r\n> Gyro::send_gyro()\r\n90,105c95,109\r\n<   if(gyro_pub_->HasConnections())\r\n<   {\r\n<     \/\/Read gyro from simulation\r\n<     float roll = this->model_->GetWorldPose().rot.GetAsEuler().x;\r\n<     float pitch = this->model_->GetWorldPose().rot.GetAsEuler().y;\r\n<     float yaw = this->model_->GetWorldPose().rot.GetAsEuler().z;\r\n< \r\n<     \/\/build message\r\n<     msgs::Vector3d gyroMsg;\r\n<     gyroMsg.set_x(roll);\r\n<     gyroMsg.set_y(pitch);\r\n<     gyroMsg.set_z(yaw);\r\n< \r\n<     \/\/send\r\n<     gyro_pub_->Publish(gyroMsg);\r\n<   }\r\n---\r\n> \tif (gyro_pub_->HasConnections()) {\r\n> \t\t\/\/Read gyro from simulation\r\n> \t\tfloat roll  = this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_EULER_X;\r\n> \t\tfloat pitch = this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_EULER_Y;\r\n> \t\tfloat yaw   = this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_EULER_Z;\r\n> \r\n> \t\t\/\/build message\r\n> \t\tmsgs::Vector3d gyroMsg;\r\n> \t\tgyroMsg.set_x(roll);\r\n> \t\tgyroMsg.set_y(pitch);\r\n> \t\tgyroMsg.set_z(yaw);\r\n> \r\n> \t\t\/\/send\r\n> \t\tgyro_pub_->Publish(gyroMsg);\r\n> \t}\r\ndiff -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\r\n21a22\r\n> #include <gazebo\/common\/common.hh>\r\n24,25d24\r\n< #include <gazebo\/common\/common.hh>\r\n< #include <stdio.h>\r\n27a27\r\n> #include <stdio.h>\r\n30,32c30,31\r\n< namespace gazebo\r\n< {   \r\n<   \/** @class Gyro\r\n---\r\n> namespace gazebo {\r\n> \/** @class Gyro\r\n36,74c35,72\r\n<   class Gyro : public ModelPlugin\r\n<   {\r\n<   public:\r\n<     \/\/\/Constructor\r\n<     Gyro();\r\n< \r\n<     \/\/\/Destructor\r\n<     ~Gyro();\r\n< \r\n<     \/\/Overridden ModelPlugin-Functions\r\n<     virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr \/*_sdf*\/);\r\n<     virtual void OnUpdate(const common::UpdateInfo &);\r\n<     virtual void Reset();\r\n< \r\n<   private:\r\n<     \/\/\/ Pointer to the model\r\n<     physics::ModelPtr model_;\r\n<     \/\/\/ Pointer to the update event connection\r\n<     event::ConnectionPtr update_connection_;\r\n<     \/\/\/Node for communication to fawkes\r\n<     transport::NodePtr node_;\r\n<     \/\/\/name of the gyro and the communication channel\r\n<     std::string name_;\r\n< \r\n<     \/\/\/time variable to send in intervals\r\n<     double last_sent_time_;\r\n< \r\n<     \/\/\/time interval between to gyro msgs\r\n<     double send_interval_;\r\n< \r\n< \r\n<     \/\/Gyro Stuff:\r\n<     \/\/\/Sending Gyro-angle to fawkes:\r\n<     void send_gyro();\r\n< \r\n<     \/\/\/Publisher for GyroAngle\r\n<     transport::PublisherPtr gyro_pub_;  \r\n<   };\r\n< }\r\n---\r\n> class Gyro : public ModelPlugin\r\n> {\r\n> public:\r\n> \t\/\/\/Constructor\r\n> \tGyro();\r\n> \r\n> \t\/\/\/Destructor\r\n> \t~Gyro();\r\n> \r\n> \t\/\/Overridden ModelPlugin-Functions\r\n> \tvirtual void Load(physics::ModelPtr _parent, sdf::ElementPtr \/*_sdf*\/);\r\n> \tvirtual void OnUpdate(const common::UpdateInfo &);\r\n> \tvirtual void Reset();\r\n> \r\n> private:\r\n> \t\/\/\/ Pointer to the model\r\n> \tphysics::ModelPtr model_;\r\n> \t\/\/\/ Pointer to the update event connection\r\n> \tevent::ConnectionPtr update_connection_;\r\n> \t\/\/\/Node for communication to fawkes\r\n> \ttransport::NodePtr node_;\r\n> \t\/\/\/name of the gyro and the communication channel\r\n> \tstd::string name_;\r\n> \r\n> \t\/\/\/time variable to send in intervals\r\n> \tdouble last_sent_time_;\r\n> \r\n> \t\/\/\/time interval between to gyro msgs\r\n> \tdouble send_interval_;\r\n> \r\n> \t\/\/Gyro Stuff:\r\n> \t\/\/\/Sending Gyro-angle to fawkes:\r\n> \tvoid send_gyro();\r\n> \r\n> \t\/\/\/Publisher for GyroAngle\r\n> \ttransport::PublisherPtr gyro_pub_;\r\n> };\r\n> } \/\/ namespace gazebo\r\ndiff -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\r\n26c26\r\n< LIBS_gazebo_libgyro = gazsim_msgs\r\n---\r\n> LIBS_gazebo_libgyro = m gazsim_msgs\r\n32c32,33\r\n< OBJS_all    = $(OBJS_gazebo_libgyro)\r\n---\r\n> OBJS_all = $(OBJS_gazebo_libgyro)\r\n> LIBS_all = $(LIBDIR)\/gazebo\/libgyro.so\r\n36c37,38\r\n<   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) -lm $(call boost-libs-ldflags,system) -lboost_system\r\n---\r\n>   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) \\\r\n>              $(call boost-libs-ldflags,system)\r\n38c40\r\n<   LIBS_all = $(LIBDIR)\/gazebo\/libgyro.so\r\n---\r\n>   LIBS_build = $(LIBS_all)\r\ndiff -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\r\n26c26\r\n< LIBS_gazebo_libmotor = gazsim_msgs\r\n---\r\n> LIBS_gazebo_libmotor = m gazsim_msgs\r\n32c32,33\r\n< OBJS_all    = $(OBJS_gazebo_libmotor)\r\n---\r\n> OBJS_all = $(OBJS_gazebo_libmotor)\r\n> LIBS_all = $(LIBDIR)\/gazebo\/libmotor.so\r\n36c37,38\r\n<   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) -lm $(call boost-libs-ldflags,system) -lboost_system\r\n---\r\n>   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) \\\r\n>              $(call boost-libs-ldflags,system)\r\n38c40\r\n<   LIBS_all = $(LIBDIR)\/gazebo\/libmotor.so\r\n---\r\n>   LIBS_build = $(LIBS_all)\r\ndiff -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\r\n21,22d20\r\n< #include <math.h>\r\n< \r\n24a23,26\r\n> #include <utils\/misc\/gazebo_api_wrappers.h>\r\n> \r\n> #include <math.h>\r\n> \r\n30c32\r\n< Motor::Motor()\r\n---\r\n> Motor::Motor() : vx_(0), vy_(0), vomega_(0)\r\n36c38\r\n<   printf("Destructing Motor Plugin!\\n");\r\n---\r\n> \tprintf("Destructing Motor Plugin!\\n");\r\n42c44,45\r\n< void Motor::Load(physics::ModelPtr _parent, sdf::ElementPtr \/*_sdf*\/) \r\n---\r\n> void\r\n> Motor::Load(physics::ModelPtr _parent, sdf::ElementPtr \/*_sdf*\/)\r\n44,49c47,48\r\n<   \/\/ Store the pointer to the model\r\n<   this->model_ = _parent;\r\n< \r\n<   \/\/get the model-name\r\n<   this->name_ = model_->GetName();\r\n<   printf("Loading Motor Plugin of model %s\\n", name_.c_str());\r\n---\r\n> \t\/\/ Store the pointer to the model\r\n> \tthis->model_ = _parent;\r\n51,67c50,72\r\n<   \/\/ Listen to the update event. This event is broadcast every\r\n<   \/\/ simulation iteration.\r\n<   this->update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&Motor::OnUpdate, this, _1));\r\n< \r\n<   \/\/Create the communication Node for communication with fawkes\r\n<   this->node_ = transport::NodePtr(new transport::Node());\r\n<   \/\/the namespace is set to the model name!\r\n<   this->node_->Init(model_->GetWorld()->GetName()+"\/"+name_);\r\n< \r\n< \r\n<   \/\/initialize movement commands:\r\n<   vx_ = 0.0;\r\n<   vy_ = 0.0;\r\n<   vomega_ = 0.0;\r\n< \r\n<   \/\/create subscriber\r\n<   this->motor_move_sub_ = this->node_->Subscribe(std::string("~\/RobotinoSim\/MotorMove\/"), &Motor::on_motor_move_msg, this);\r\n---\r\n> \t\/\/get the model-name\r\n> \tthis->name_ = model_->GetName();\r\n> \tprintf("Loading Motor Plugin of model %s\\n", name_.c_str());\r\n> \r\n> \t\/\/ Listen to the update event. This event is broadcast every\r\n> \t\/\/ simulation iteration.\r\n> \tthis->update_connection_ =\r\n> \t  event::Events::ConnectWorldUpdateBegin(boost::bind(&Motor::OnUpdate, this, _1));\r\n> \r\n> \t\/\/Create the communication Node for communication with fawkes\r\n> \tthis->node_ = transport::NodePtr(new transport::Node());\r\n> \t\/\/the namespace is set to the model name!\r\n> \tthis->node_->Init(model_->GetWorld()->GZWRAP_NAME() + "\/" + name_);\r\n> \r\n> \t\/\/initialize movement commands:\r\n> \tvx_     = 0.0;\r\n> \tvy_     = 0.0;\r\n> \tvomega_ = 0.0;\r\n> \r\n> \t\/\/create subscriber\r\n> \tthis->motor_move_sub_ = this->node_->Subscribe(std::string("~\/RobotinoSim\/MotorMove\/"),\r\n> \t                                               &Motor::on_motor_move_msg,\r\n> \t                                               this);\r\n72c77,78\r\n< void Motor::OnUpdate(const common::UpdateInfo & \/*_info*\/)\r\n---\r\n> void\r\n> Motor::OnUpdate(const common::UpdateInfo & \/*_info*\/)\r\n74,85c80,91\r\n<   \/\/Apply movement command\r\n<   float x,y;\r\n<   float yaw = this->model_->GetWorldPose().rot.GetAsEuler().z;\r\n<   \/\/foward part\r\n<   x = cos(yaw) * vx_;\r\n<   y = sin(yaw) * vx_;\r\n<   \/\/sideways part\r\n<   x += cos(yaw + 3.1415926f \/ 2) * vy_;\r\n<   y += sin(yaw + 3.1415926f \/ 2) * vy_;\r\n<   \/\/ Apply velocity to the model.\r\n<   this->model_->SetLinearVel(math::Vector3(x, y, 0));\r\n<   this->model_->SetAngularVel(math::Vector3(0, 0, vomega_));\r\n---\r\n> \t\/\/Apply movement command\r\n> \tfloat x, y;\r\n> \tfloat yaw = this->model_->GZWRAP_WORLD_POSE().GZWRAP_ROT_EULER_Z;\r\n> \t\/\/foward part\r\n> \tx = cos(yaw) * vx_;\r\n> \ty = sin(yaw) * vx_;\r\n> \t\/\/sideways part\r\n> \tx += cos(yaw + 3.1415926f \/ 2) * vy_;\r\n> \ty += sin(yaw + 3.1415926f \/ 2) * vy_;\r\n> \t\/\/ Apply velocity to the model.\r\n> \tthis->model_->SetLinearVel(gzwrap::Vector3d(x, y, 0));\r\n> \tthis->model_->SetAngularVel(gzwrap::Vector3d(0, 0, vomega_));\r\n90c96,97\r\n< void Motor::Reset()\r\n---\r\n> void\r\n> Motor::Reset()\r\n96,97c103,105\r\n<  *\/ \r\n< void Motor::on_motor_move_msg(ConstVector3dPtr &msg)\r\n---\r\n>  *\/\r\n> void\r\n> Motor::on_motor_move_msg(ConstVector3dPtr &msg)\r\n99,103c107,111\r\n<   \/\/printf("Got MotorMove Msg!!! %f %f %f\\n", msg->x(), msg->y(), msg->z());\r\n<   \/\/Transform relative motion into ablosulte motion\r\n<   vx_ = msg->x();\r\n<   vy_ = msg->y();\r\n<   vomega_ = msg->z();\r\n---\r\n> \t\/\/printf("Got MotorMove Msg!!! %f %f %f\\n", msg->x(), msg->y(), msg->z());\r\n> \t\/\/Transform relative motion into ablosulte motion\r\n> \tvx_     = msg->x();\r\n> \tvy_     = msg->y();\r\n> \tvomega_ = msg->z();\r\ndiff -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\r\n21a22\r\n> #include <gazebo\/common\/common.hh>\r\n24,25d24\r\n< #include <gazebo\/common\/common.hh>\r\n< #include <stdio.h>\r\n27a27\r\n> #include <stdio.h>\r\n30,32c30,31\r\n< namespace gazebo\r\n< {   \r\n<   \/** @class Motor\r\n---\r\n> namespace gazebo {\r\n> \/** @class Motor\r\n36,73c35,70\r\n<   class Motor : public ModelPlugin\r\n<   {\r\n<   public:\r\n<     \/\/\/Constructor\r\n<     Motor();\r\n< \r\n<     \/\/\/Destructor\r\n<     ~Motor();\r\n< \r\n<     \/\/Overridden ModelPlugin-Functions\r\n<     virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr \/*_sdf*\/);\r\n<     virtual void OnUpdate(const common::UpdateInfo &);\r\n<     virtual void Reset();\r\n< \r\n<   private:\r\n<     \/\/\/ Pointer to the Gazebo model\r\n<     physics::ModelPtr model_;\r\n<     \/\/\/ Pointer to the update event connection\r\n<     event::ConnectionPtr update_connection_;\r\n<     \/\/\/Node for communication to fawkes\r\n<     transport::NodePtr node_;\r\n<     \/\/\/name of the motor and the communication channel\r\n<     std::string name_;\r\n< \r\n< \r\n<     \/\/Motor Stuff:\r\n<     void on_motor_move_msg(ConstVector3dPtr &msg);\r\n< \r\n<     \/\/\/Suscriber for MotorMove Interfaces from Fawkes\r\n<     transport::SubscriberPtr motor_move_sub_;\r\n< \r\n< \r\n<     \/\/current movement commands:\r\n<     float vx_;\r\n<     float vy_;\r\n<     float vomega_;  \r\n<   };\r\n< }\r\n---\r\n> class Motor : public ModelPlugin\r\n> {\r\n> public:\r\n> \t\/\/\/Constructor\r\n> \tMotor();\r\n> \r\n> \t\/\/\/Destructor\r\n> \t~Motor();\r\n> \r\n> \t\/\/Overridden ModelPlugin-Functions\r\n> \tvirtual void Load(physics::ModelPtr _parent, sdf::ElementPtr \/*_sdf*\/);\r\n> \tvirtual void OnUpdate(const common::UpdateInfo &);\r\n> \tvirtual void Reset();\r\n> \r\n> private:\r\n> \t\/\/\/ Pointer to the Gazebo model\r\n> \tphysics::ModelPtr model_;\r\n> \t\/\/\/ Pointer to the update event connection\r\n> \tevent::ConnectionPtr update_connection_;\r\n> \t\/\/\/Node for communication to fawkes\r\n> \ttransport::NodePtr node_;\r\n> \t\/\/\/name of the motor and the communication channel\r\n> \tstd::string name_;\r\n> \r\n> \t\/\/Motor Stuff:\r\n> \tvoid on_motor_move_msg(ConstVector3dPtr &msg);\r\n> \r\n> \t\/\/\/Suscriber for MotorMove Interfaces from Fawkes\r\n> \ttransport::SubscriberPtr motor_move_sub_;\r\n> \r\n> \t\/\/current movement commands:\r\n> \tfloat vx_;\r\n> \tfloat vy_;\r\n> \tfloat vomega_;\r\n> };\r\n> } \/\/ namespace gazebo\r\ndiff -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\r\n21,22d20\r\n< #include <core\/plugin.h>\r\n< \r\n24a23,24\r\n> #include <core\/plugin.h>\r\n> \r\n34,36c34,35\r\n<  public:\r\n< \r\n<   \/** Constructor.\r\n---\r\n> public:\r\n> \t\/** Constructor.\r\n39,43c38,41\r\n<   GazsimRobotinoPlugin(Configuration *config)\r\n<     : Plugin(config)\r\n<   {\r\n<     thread_list.push_back(new RobotinoSimThread());\r\n<   }\r\n---\r\n> \texplicit GazsimRobotinoPlugin(Configuration *config) : Plugin(config)\r\n> \t{\r\n> \t\tthread_list.push_back(new RobotinoSimThread());\r\n> \t}\r\ndiff -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\r\n23,28d22\r\n< #include <tf\/types.h>\r\n< #include <core\/threading\/mutex_locker.h>\r\n< \r\n< #include <gazebo\/transport\/Node.hh>\r\n< #include <gazebo\/msgs\/msgs.hh>\r\n< #include <gazebo\/transport\/transport.hh>\r\n30c24,25\r\n< \r\n---\r\n> #include <core\/threading\/mutex_locker.h>\r\n> #include <interfaces\/IMUInterface.h>\r\n34,35d28\r\n< #include <interfaces\/IMUInterface.h>\r\n< \r\n37c30\r\n< #include <utils\/time\/clock.h>\r\n---\r\n> #include <tf\/types.h>\r\n38a32\r\n> #include <utils\/time\/clock.h>\r\n40a35,37\r\n> #include <gazebo\/msgs\/msgs.hh>\r\n> #include <gazebo\/transport\/Node.hh>\r\n> #include <gazebo\/transport\/transport.hh>\r\n55,57c52,54\r\n<   : Thread("RobotinoSimThread", Thread::OPMODE_WAITFORWAKEUP),\r\n<     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE), \/\/sonsor and act here\r\n<     TransformAspect(TransformAspect::DEFER_PUBLISHER)\r\n---\r\n> : Thread("RobotinoSimThread", Thread::OPMODE_WAITFORWAKEUP),\r\n>   BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE), \/\/sonsor and act here\r\n>   TransformAspect(TransformAspect::DEFER_PUBLISHER)\r\n64,146c61,62\r\n<   \/\/get a connection to gazebo (copied from gazeboscene)\r\n<   logger->log_debug(name(), "Creating Gazebo publishers");\r\n< \r\n<   \/\/read config values\r\n<   cfg_frame_odom_ = config->get_string("\/frames\/odom");\r\n<   cfg_frame_base_ = config->get_string("\/frames\/base");\r\n<   slippery_wheels_enabled_ = config->get_bool("gazsim\/robotino\/motor\/slippery-wheels-enabled");\r\n<   slippery_wheels_threshold_ = config->get_float("gazsim\/robotino\/motor\/slippery-wheels-threshold");\r\n<   moving_speed_factor_ = config->get_float("gazsim\/robotino\/motor\/moving-speed-factor");\r\n<   rotation_speed_factor_ = config->get_float("gazsim\/robotino\/motor\/rotation-speed-factor");\r\n<   gripper_laser_threshold_ = config->get_float("\/gazsim\/robotino\/gripper-laser-threshold");\r\n<   gripper_laser_value_far_ = config->get_float("\/gazsim\/robotino\/gripper-laser-value-far");\r\n<   gripper_laser_value_near_ = config->get_float("\/gazsim\/robotino\/gripper-laser-value-near");\r\n<   have_gripper_sensors_ = config->exists("\/hardware\/robotino\/sensors\/right_ir_id")\r\n<     && config->exists("\/hardware\/robotino\/sensors\/left_ir_id");\r\n<   if(have_gripper_sensors_)\r\n<   {\r\n<     gripper_laser_right_pos_ = config->get_int("\/hardware\/robotino\/sensors\/right_ir_id");\r\n<     gripper_laser_left_pos_ = config->get_int("\/hardware\/robotino\/sensors\/left_ir_id");\r\n<   }\r\n<   gyro_buffer_size_ = config->get_int("\/gazsim\/robotino\/gyro-buffer-size");\r\n<   gyro_delay_ = config->get_float("\/gazsim\/robotino\/gyro-delay");\r\n<   infrared_sensor_index_ = config->get_int("\/gazsim\/robotino\/infrared-sensor-index");\r\n< \r\n<   tf_enable_publisher(cfg_frame_base_.c_str());\r\n<  \r\n<   \/\/Open interfaces\r\n<   motor_if_  = blackboard->open_for_writing<MotorInterface>("Robotino");\r\n<   switch_if_ = blackboard->open_for_writing<fawkes::SwitchInterface>("Robotino Motor");\r\n<   sens_if_   = blackboard->open_for_writing<RobotinoSensorInterface>("Robotino");\r\n<   imu_if_    = blackboard->open_for_writing<IMUInterface>("IMU Robotino");\r\n< \r\n<   \/\/Create suscribers\r\n<   pos_sub_ = gazebonode->Subscribe(config->get_string("\/gazsim\/topics\/gps"), &RobotinoSimThread::on_pos_msg, this);\r\n<   gyro_sub_ = gazebonode->Subscribe(config->get_string("\/gazsim\/topics\/gyro"), &RobotinoSimThread::on_gyro_msg, this);\r\n<   infrared_puck_sensor_sub_ = gazebonode->Subscribe(config->get_string("\/gazsim\/topics\/infrared-puck-sensor"), &RobotinoSimThread::on_infrared_puck_sensor_msg, this);\r\n<   if(have_gripper_sensors_)\r\n<   {\r\n<     gripper_laser_left_sensor_sub_ = gazebonode->Subscribe(config->get_string("\/gazsim\/topics\/gripper-laser-left"), &RobotinoSimThread::on_gripper_laser_left_sensor_msg, this);\r\n<     gripper_laser_right_sensor_sub_ = gazebonode->Subscribe(config->get_string("\/gazsim\/topics\/gripper-laser-right"), &RobotinoSimThread::on_gripper_laser_right_sensor_msg, this);\r\n<   }\r\n< \r\n<   \/\/Create publishers\r\n<   motor_move_pub_ = gazebonode->Advertise<msgs::Vector3d>(config->get_string("\/gazsim\/topics\/motor-move"));\r\n<   string_pub_ = gazebonode->Advertise<msgs::Header>(config->get_string("\/gazsim\/topics\/message"));\r\n< \r\n<   \/\/enable motor by default\r\n<   switch_if_->set_enabled(true);\r\n<   switch_if_->write();\r\n< \r\n<   imu_if_->set_linear_acceleration(0, -1.);\r\n<   \/\/imu_if_->set_angular_velocity_covariance(8, deg2rad(0.1));\r\n<   \/\/ set as not available as we do not currently provide angular velocities.\r\n<   imu_if_->set_angular_velocity_covariance(0, -1.);\r\n<   imu_if_->write();\r\n< \r\n<   \/\/init motor variables\r\n<   x_ = 0.0;\r\n<   y_ = 0.0;\r\n<   ori_ = 0.0;\r\n<   vx_ = 0.0;\r\n<   vy_ = 0.0;\r\n<   vomega_ = 0.0;\r\n<   des_vx_ = 0.0;\r\n<   des_vy_ = 0.0;\r\n<   des_vomega_ = 0.0;\r\n<   x_offset_ = 0.0;\r\n<   y_offset_ = 0.0;\r\n<   ori_offset_ = 0.0;\r\n<   path_length_ = 0.0;\r\n< \r\n<   gyro_buffer_index_new_ = 0;\r\n<   gyro_buffer_index_delayed_ = 0;\r\n<   gyro_timestamp_buffer_ = new fawkes::Time[gyro_buffer_size_];\r\n<   gyro_angle_buffer_ = new float[gyro_buffer_size_];\r\n< \r\n<   new_data_ = false;\r\n< \r\n<   if(string_pub_->HasConnections())\r\n<   {\r\n<     msgs::Header helloMessage;\r\n<     helloMessage.set_str_id("gazsim-robotino plugin connected");\r\n<     string_pub_->Publish(helloMessage);\r\n---\r\n> \t\/\/get a connection to gazebo (copied from gazeboscene)\r\n> \tlogger->log_debug(name(), "Creating Gazebo publishers");\r\n148c64,157\r\n<   }\r\n---\r\n> \t\/\/read config values\r\n> \tcfg_frame_odom_            = config->get_string("\/frames\/odom");\r\n> \tcfg_frame_base_            = config->get_string("\/frames\/base");\r\n> \tcfg_frame_imu_             = config->get_string("\/gazsim\/robotino\/imu\/frame");\r\n> \tslippery_wheels_enabled_   = config->get_bool("gazsim\/robotino\/motor\/slippery-wheels-enabled");\r\n> \tslippery_wheels_threshold_ = config->get_float("gazsim\/robotino\/motor\/slippery-wheels-threshold");\r\n> \tmoving_speed_factor_       = config->get_float("gazsim\/robotino\/motor\/moving-speed-factor");\r\n> \trotation_speed_factor_     = config->get_float("gazsim\/robotino\/motor\/rotation-speed-factor");\r\n> \tgripper_laser_threshold_   = config->get_float("\/gazsim\/robotino\/gripper-laser-threshold");\r\n> \tgripper_laser_value_far_   = config->get_float("\/gazsim\/robotino\/gripper-laser-value-far");\r\n> \tgripper_laser_value_near_  = config->get_float("\/gazsim\/robotino\/gripper-laser-value-near");\r\n> \thave_gripper_sensors_      = config->exists("\/hardware\/robotino\/sensors\/right_ir_id")\r\n> \t                        && config->exists("\/hardware\/robotino\/sensors\/left_ir_id");\r\n> \tif (have_gripper_sensors_) {\r\n> \t\tgripper_laser_right_pos_ = config->get_int("\/hardware\/robotino\/sensors\/right_ir_id");\r\n> \t\tgripper_laser_left_pos_  = config->get_int("\/hardware\/robotino\/sensors\/left_ir_id");\r\n> \t}\r\n> \tgyro_buffer_size_      = config->get_int("\/gazsim\/robotino\/gyro-buffer-size");\r\n> \tgyro_delay_            = config->get_float("\/gazsim\/robotino\/gyro-delay");\r\n> \tinfrared_sensor_index_ = config->get_int("\/gazsim\/robotino\/infrared-sensor-index");\r\n> \r\n> \ttf_enable_publisher(cfg_frame_base_.c_str());\r\n> \r\n> \t\/\/Open interfaces\r\n> \tmotor_if_  = blackboard->open_for_writing<MotorInterface>("Robotino");\r\n> \tswitch_if_ = blackboard->open_for_writing<fawkes::SwitchInterface>("Robotino Motor");\r\n> \tsens_if_   = blackboard->open_for_writing<RobotinoSensorInterface>("Robotino");\r\n> \timu_if_    = blackboard->open_for_writing<IMUInterface>("IMU Robotino");\r\n> \r\n> \t\/\/Create suscribers\r\n> \tpos_sub_  = gazebonode->Subscribe(config->get_string("\/gazsim\/topics\/gps"),\r\n>                                    &RobotinoSimThread::on_pos_msg,\r\n>                                    this);\r\n> \tgyro_sub_ = gazebonode->Subscribe(config->get_string("\/gazsim\/topics\/gyro"),\r\n> \t                                  &RobotinoSimThread::on_gyro_msg,\r\n> \t                                  this);\r\n> \tinfrared_puck_sensor_sub_ =\r\n> \t  gazebonode->Subscribe(config->get_string("\/gazsim\/topics\/infrared-puck-sensor"),\r\n> \t                        &RobotinoSimThread::on_infrared_puck_sensor_msg,\r\n> \t                        this);\r\n> \tif (have_gripper_sensors_) {\r\n> \t\tgripper_laser_left_sensor_sub_ =\r\n> \t\t  gazebonode->Subscribe(config->get_string("\/gazsim\/topics\/gripper-laser-left"),\r\n> \t\t                        &RobotinoSimThread::on_gripper_laser_left_sensor_msg,\r\n> \t\t                        this);\r\n> \t\tgripper_laser_right_sensor_sub_ =\r\n> \t\t  gazebonode->Subscribe(config->get_string("\/gazsim\/topics\/gripper-laser-right"),\r\n> \t\t                        &RobotinoSimThread::on_gripper_laser_right_sensor_msg,\r\n> \t\t                        this);\r\n> \t}\r\n> \r\n> \t\/\/Create publishers\r\n> \tmotor_move_pub_ =\r\n> \t  gazebonode->Advertise<msgs::Vector3d>(config->get_string("\/gazsim\/topics\/motor-move"));\r\n> \tstring_pub_ = gazebonode->Advertise<msgs::Header>(config->get_string("\/gazsim\/topics\/message"));\r\n> \r\n> \t\/\/enable motor by default\r\n> \tswitch_if_->set_enabled(true);\r\n> \tswitch_if_->write();\r\n> \r\n> \timu_if_->set_frame(cfg_frame_imu_.c_str());\r\n> \timu_if_->set_linear_acceleration(0, -1.);\r\n> \t\/\/imu_if_->set_angular_velocity_covariance(8, deg2rad(0.1));\r\n> \t\/\/ set as not available as we do not currently provide angular velocities.\r\n> \timu_if_->set_angular_velocity_covariance(0, -1.);\r\n> \timu_if_->write();\r\n> \r\n> \t\/\/init motor variables\r\n> \tx_           = 0.0;\r\n> \ty_           = 0.0;\r\n> \tori_         = 0.0;\r\n> \tvx_          = 0.0;\r\n> \tvy_          = 0.0;\r\n> \tvomega_      = 0.0;\r\n> \tdes_vx_      = 0.0;\r\n> \tdes_vy_      = 0.0;\r\n> \tdes_vomega_  = 0.0;\r\n> \tx_offset_    = 0.0;\r\n> \ty_offset_    = 0.0;\r\n> \tori_offset_  = 0.0;\r\n> \tpath_length_ = 0.0;\r\n> \r\n> \tgyro_buffer_index_new_     = 0;\r\n> \tgyro_buffer_index_delayed_ = 0;\r\n> \tgyro_timestamp_buffer_     = new fawkes::Time[gyro_buffer_size_];\r\n> \tgyro_angle_buffer_         = new float[gyro_buffer_size_];\r\n> \r\n> \tnew_data_ = false;\r\n> \r\n> \tif (string_pub_->HasConnections()) {\r\n> \t\tmsgs::Header helloMessage;\r\n> \t\thelloMessage.set_str_id("gazsim-robotino plugin connected");\r\n> \t\tstring_pub_->Publish(helloMessage);\r\n> \t}\r\n154,158c163,167\r\n<   \/\/close interfaces\r\n<   blackboard->close(imu_if_);\r\n<   blackboard->close(sens_if_);\r\n<   blackboard->close(motor_if_);\r\n<   blackboard->close(switch_if_);\r\n---\r\n> \t\/\/close interfaces\r\n> \tblackboard->close(imu_if_);\r\n> \tblackboard->close(sens_if_);\r\n> \tblackboard->close(motor_if_);\r\n> \tblackboard->close(switch_if_);\r\n160,161c169,170\r\n<   delete [] gyro_timestamp_buffer_;\r\n<   delete [] gyro_angle_buffer_;\r\n---\r\n> \tdelete[] gyro_timestamp_buffer_;\r\n> \tdelete[] gyro_angle_buffer_;\r\n167,168c176,227\r\n<   \/\/work off all messages passed to the motor_interfaces\r\n<   process_motor_messages();\r\n---\r\n> \t\/\/work off all messages passed to the motor_interfaces\r\n> \tprocess_motor_messages();\r\n> \r\n> \t\/\/update interfaces\r\n> \tif (new_data_) {\r\n> \t\tmotor_if_->set_odometry_position_x(x_);\r\n> \t\tmotor_if_->set_odometry_position_y(y_);\r\n> \t\tmotor_if_->set_odometry_orientation(ori_);\r\n> \t\tmotor_if_->set_odometry_path_length(path_length_);\r\n> \t\tmotor_if_->write();\r\n> \r\n> \t\tif (gyro_available_) {\r\n> \t\t\t\/\/update gyro (with delay)\r\n> \t\t\tfawkes::Time now(clock);\r\n> \t\t\twhile ((now - gyro_timestamp_buffer_[(gyro_buffer_index_delayed_ + 1) % gyro_buffer_size_])\r\n> \t\t\t           .in_sec()\r\n> \t\t\t         >= gyro_delay_\r\n> \t\t\t       && gyro_buffer_index_delayed_ < gyro_buffer_index_new_) {\r\n> \t\t\t\tgyro_buffer_index_delayed_++;\r\n> \t\t\t}\r\n> \r\n> \t\t\ttf::Quaternion q =\r\n> \t\t\t  tf::create_quaternion_from_yaw(gyro_angle_buffer_[gyro_buffer_index_delayed_]);\r\n> \t\t\timu_if_->set_orientation(0, q.x());\r\n> \t\t\timu_if_->set_orientation(1, q.y());\r\n> \t\t\timu_if_->set_orientation(2, q.z());\r\n> \t\t\timu_if_->set_orientation(3, q.w());\r\n> \t\t\tfor (uint i = 0; i < 9u; i += 4) {\r\n> \t\t\t\timu_if_->set_orientation_covariance(i, 1e-3);\r\n> \t\t\t\timu_if_->set_angular_velocity_covariance(i, 1e-3);\r\n> \t\t\t\timu_if_->set_linear_acceleration_covariance(i, 1e-3);\r\n> \t\t\t}\r\n> \t\t} else {\r\n> \t\t\timu_if_->set_angular_velocity(0, -1.);\r\n> \t\t\timu_if_->set_orientation(0, -1.);\r\n> \t\t\timu_if_->set_orientation(1, 0.);\r\n> \t\t\timu_if_->set_orientation(2, 0.);\r\n> \t\t\timu_if_->set_orientation(3, 0.);\r\n> \t\t}\r\n> \t\timu_if_->write();\r\n> \r\n> \t\tsens_if_->set_distance(infrared_sensor_index_, infrared_puck_sensor_dist_);\r\n> \r\n> \t\tif (have_gripper_sensors_) {\r\n> \t\t\tsens_if_->set_analog_in(gripper_laser_left_pos_, analog_in_left_);\r\n> \t\t\tsens_if_->set_analog_in(gripper_laser_right_pos_, analog_in_right_);\r\n> \t\t}\r\n> \t\tsens_if_->write();\r\n> \r\n> \t\tnew_data_ = false;\r\n> \t}\r\n> }\r\n170,292c229,236\r\n<   \/\/update interfaces\r\n<   if(new_data_)\r\n<   {\r\n<     motor_if_->set_odometry_position_x(x_);\r\n<     motor_if_->set_odometry_position_y(y_);\r\n<     motor_if_->set_odometry_orientation(ori_);\r\n<     motor_if_->set_odometry_path_length(path_length_);\r\n<     motor_if_->write();\r\n< \r\n<     if(gyro_available_)\r\n<     {\r\n<       \/\/update gyro (with delay)\r\n<       fawkes::Time now(clock);\r\n<       while ((now - gyro_timestamp_buffer_[(gyro_buffer_index_delayed_ + 1) % gyro_buffer_size_]).in_sec() >= gyro_delay_\r\n< \t     && gyro_buffer_index_delayed_ < gyro_buffer_index_new_)\r\n<       {\r\n< \tgyro_buffer_index_delayed_++;\r\n<       }\r\n< \t  \r\n<       tf::Quaternion q =\r\n< \ttf::create_quaternion_from_yaw(gyro_angle_buffer_[gyro_buffer_index_delayed_]);\r\n<       imu_if_->set_orientation(0, q.x());\r\n<       imu_if_->set_orientation(1, q.y());\r\n<       imu_if_->set_orientation(2, q.z());\r\n<       imu_if_->set_orientation(3, q.w());\r\n<     } else {\r\n<       imu_if_->set_angular_velocity(0, -1.);\r\n<       imu_if_->set_orientation(0, -1.);\r\n<       imu_if_->set_orientation(1,  0.);\r\n<       imu_if_->set_orientation(2,  0.);\r\n<       imu_if_->set_orientation(3,  0.);\r\n<     }\r\n<     imu_if_->write();\r\n< \r\n<     sens_if_->set_distance(infrared_sensor_index_, infrared_puck_sensor_dist_);\r\n<     \r\n<     if(have_gripper_sensors_)\r\n<     {\r\n<       sens_if_->set_analog_in(gripper_laser_left_pos_, analog_in_left_);\r\n<       sens_if_->set_analog_in(gripper_laser_right_pos_, analog_in_right_);\r\n<     }    \r\n<     sens_if_->write();\r\n< \r\n<     new_data_ = false;\r\n<   }\r\n< }\r\n< \r\n< void RobotinoSimThread::send_transroot(double vx, double vy, double omega)\r\n< {\r\n<   msgs::Vector3d motorMove;\r\n<   motorMove.set_x(vx_);\r\n<   motorMove.set_y(vy_);\r\n<   motorMove.set_z(vomega_);\r\n<   motor_move_pub_->Publish(motorMove);\r\n< }\r\n< \r\n< void RobotinoSimThread::process_motor_messages()\r\n< {\r\n<   \/\/check messages of the switch interface\r\n<   while (!switch_if_->msgq_empty()) {\r\n<     if (SwitchInterface::DisableSwitchMessage *msg =\r\n< \tswitch_if_->msgq_first_safe(msg)) {\r\n<       switch_if_->set_enabled(false);\r\n<       \/\/pause movement\r\n<       send_transroot(0, 0, 0);\r\n<     } else if (SwitchInterface::EnableSwitchMessage *msg =\r\n< \t       switch_if_->msgq_first_safe(msg)) {\r\n<       switch_if_->set_enabled(true);\r\n<       \/\/unpause movement\r\n<       send_transroot(vx_, vy_, vomega_);\r\n<     }\r\n<     switch_if_->msgq_pop();\r\n<     switch_if_->write();\r\n<   }\r\n< \r\n<   \/\/do not do anything if the motor is disabled\r\n<   if(!switch_if_->is_enabled())\r\n<   {\r\n<     return;\r\n<   }\r\n< \r\n<   \/\/check messages of the motor interface\r\n<   while(motor_move_pub_->HasConnections() && !motor_if_->msgq_empty())\r\n<   {\r\n<     if (MotorInterface::TransRotMessage *msg =\r\n< \tmotor_if_->msgq_first_safe(msg))\r\n<     {\r\n<       \/\/send command only if changed\r\n<       if(vel_changed(msg->vx(), vx_, 0.01) || vel_changed(msg->vy(), vy_, 0.01) || vel_changed(msg->omega(), vomega_, 0.01))\r\n<       {\r\n< \tvx_ = msg->vx();\r\n< \tvy_ = msg->vy();\r\n< \tvomega_ = msg->omega();\r\n< \tdes_vx_ = vx_;\r\n< \tdes_vy_ = vy_;\r\n< \tdes_vomega_ = vomega_;\r\n< \t\r\n< \t\/\/send message to gazebo (apply movement_factor to compensate friction)\r\n< \tsend_transroot(vx_ * moving_speed_factor_, vy_ * moving_speed_factor_, vomega_ * rotation_speed_factor_);\r\n< \r\n< \t\/\/update interface\r\n< \tmotor_if_->set_vx(vx_);\r\n< \tmotor_if_->set_vy(vy_);\r\n< \tmotor_if_->set_omega(vomega_);\r\n< \tmotor_if_->set_des_vx(des_vx_);\r\n< \tmotor_if_->set_des_vy(des_vy_);\r\n< \tmotor_if_->set_des_omega(des_vomega_);\r\n< \t\/\/update interface\r\n< \tmotor_if_->write();\r\n<       }    \r\n<     }\r\n<     else if (motor_if_->msgq_first_is<MotorInterface::ResetOdometryMessage>())\r\n<       {\r\n<         x_offset_ += x_;\r\n< \ty_offset_ += y_;\r\n<         ori_offset_ += ori_;\r\n< \tx_ = 0.0;\r\n< \ty_ = 0.0;\r\n< \tori_ = 0.0;\r\n< \tlast_vel_set_time_ = clock->now();\r\n<       }\r\n<     motor_if_->msgq_pop();\r\n<   }\r\n---\r\n> void\r\n> RobotinoSimThread::send_transroot(double vx, double vy, double omega)\r\n> {\r\n> \tmsgs::Vector3d motorMove;\r\n> \tmotorMove.set_x(vx_);\r\n> \tmotorMove.set_y(vy_);\r\n> \tmotorMove.set_z(vomega_);\r\n> \tmotor_move_pub_->Publish(motorMove);\r\n295c239,240\r\n< bool RobotinoSimThread::vel_changed(float before, float after, float relativeThreashold)\r\n---\r\n> void\r\n> RobotinoSimThread::process_motor_messages()\r\n297c242,300\r\n<   return(before == 0.0 || after == 0.0 || fabs((before-after)\/before) > relativeThreashold);\r\n---\r\n> \t\/\/check messages of the switch interface\r\n> \twhile (!switch_if_->msgq_empty()) {\r\n> \t\tif (SwitchInterface::DisableSwitchMessage *msg = switch_if_->msgq_first_safe(msg)) {\r\n> \t\t\tswitch_if_->set_enabled(false);\r\n> \t\t\t\/\/pause movement\r\n> \t\t\tsend_transroot(0, 0, 0);\r\n> \t\t} else if (SwitchInterface::EnableSwitchMessage *msg = switch_if_->msgq_first_safe(msg)) {\r\n> \t\t\tswitch_if_->set_enabled(true);\r\n> \t\t\t\/\/unpause movement\r\n> \t\t\tsend_transroot(vx_, vy_, vomega_);\r\n> \t\t}\r\n> \t\tswitch_if_->msgq_pop();\r\n> \t\tswitch_if_->write();\r\n> \t}\r\n> \r\n> \t\/\/do not do anything if the motor is disabled\r\n> \tif (!switch_if_->is_enabled()) {\r\n> \t\treturn;\r\n> \t}\r\n> \r\n> \t\/\/check messages of the motor interface\r\n> \twhile (motor_move_pub_->HasConnections() && !motor_if_->msgq_empty()) {\r\n> \t\tif (MotorInterface::TransRotMessage *msg = motor_if_->msgq_first_safe(msg)) {\r\n> \t\t\t\/\/send command only if changed\r\n> \t\t\tif (vel_changed(msg->vx(), vx_, 0.01) || vel_changed(msg->vy(), vy_, 0.01)\r\n> \t\t\t    || vel_changed(msg->omega(), vomega_, 0.01)) {\r\n> \t\t\t\tvx_         = msg->vx();\r\n> \t\t\t\tvy_         = msg->vy();\r\n> \t\t\t\tvomega_     = msg->omega();\r\n> \t\t\t\tdes_vx_     = vx_;\r\n> \t\t\t\tdes_vy_     = vy_;\r\n> \t\t\t\tdes_vomega_ = vomega_;\r\n> \r\n> \t\t\t\t\/\/send message to gazebo (apply movement_factor to compensate friction)\r\n> \t\t\t\tsend_transroot(vx_ * moving_speed_factor_,\r\n> \t\t\t\t               vy_ * moving_speed_factor_,\r\n> \t\t\t\t               vomega_ * rotation_speed_factor_);\r\n> \r\n> \t\t\t\t\/\/update interface\r\n> \t\t\t\tmotor_if_->set_vx(vx_);\r\n> \t\t\t\tmotor_if_->set_vy(vy_);\r\n> \t\t\t\tmotor_if_->set_omega(vomega_);\r\n> \t\t\t\tmotor_if_->set_des_vx(des_vx_);\r\n> \t\t\t\tmotor_if_->set_des_vy(des_vy_);\r\n> \t\t\t\tmotor_if_->set_des_omega(des_vomega_);\r\n> \t\t\t\t\/\/update interface\r\n> \t\t\t\tmotor_if_->write();\r\n> \t\t\t}\r\n> \t\t} else if (motor_if_->msgq_first_is<MotorInterface::ResetOdometryMessage>()) {\r\n> \t\t\tx_offset_ += x_;\r\n> \t\t\ty_offset_ += y_;\r\n> \t\t\tori_offset_ += ori_;\r\n> \t\t\tx_                 = 0.0;\r\n> \t\t\ty_                 = 0.0;\r\n> \t\t\tori_               = 0.0;\r\n> \t\t\tlast_vel_set_time_ = clock->now();\r\n> \t\t}\r\n> \t\tmotor_if_->msgq_pop();\r\n> \t}\r\n299a303,307\r\n> bool\r\n> RobotinoSimThread::vel_changed(float before, float after, float relativeThreashold)\r\n> {\r\n> \treturn (before == 0.0 || after == 0.0 || fabs((before - after) \/ before) > relativeThreashold);\r\n> }\r\n302c310,311\r\n< void RobotinoSimThread::on_pos_msg(ConstPosePtr &msg)\r\n---\r\n> void\r\n> RobotinoSimThread::on_pos_msg(ConstPosePtr &msg)\r\n304c313\r\n<   \/\/logger_->log_debug(name_, "Got Position MSG from gazebo with ori: %f", msg->z());\r\n---\r\n> \t\/\/logger_->log_debug(name_, "Got Position MSG from gazebo with ori: %f", msg->z());\r\n306c315,403\r\n<   MutexLocker lock(loop_mutex);\r\n---\r\n> \tMutexLocker lock(loop_mutex);\r\n> \r\n> \t\/\/read out values + substract offset\r\n> \tfloat new_x = msg->position().x() - x_offset_;\r\n> \tfloat new_y = msg->position().y() - y_offset_;\r\n> \t\/\/calculate ori from quaternion\r\n> \tfloat new_ori = tf::get_yaw(tf::Quaternion(msg->orientation().x(),\r\n> \t                                           msg->orientation().y(),\r\n> \t                                           msg->orientation().z(),\r\n> \t                                           msg->orientation().w()));\r\n> \tnew_ori -= ori_offset_;\r\n> \r\n> \t\/\/estimate path-length\r\n> \tfloat length_driven = sqrt((new_x - x_) * (new_x - x_) + (new_y - y_) * (new_y - y_));\r\n> \r\n> \tif (slippery_wheels_enabled_) {\r\n> \t\t\/\/simulate slipping wheels when driving against an obstacle\r\n> \t\tfawkes::Time new_time = clock->now();\r\n> \t\tdouble       duration = new_time.in_sec() - last_pos_time_.in_sec();\r\n> \t\t\/\/calculate duration since the velocity was last set to filter slipping while accelerating\r\n> \t\tdouble velocity_set_duration = new_time.in_sec() - last_vel_set_time_.in_sec();\r\n> \r\n> \t\tlast_pos_time_ = new_time;\r\n> \r\n> \t\tdouble total_speed = sqrt(vx_ * vx_ + vy_ * vy_);\r\n> \t\tif (length_driven < total_speed * duration * slippery_wheels_threshold_\r\n> \t\t    && velocity_set_duration > duration) {\r\n> \t\t\tdouble speed_abs_x = vx_ * cos(ori_) - vy_ * sin(ori_);\r\n> \t\t\tdouble speed_abs_y = vx_ * sin(ori_) + vy_ * cos(ori_);\r\n> \t\t\tdouble slipped_x   = speed_abs_x * duration * slippery_wheels_threshold_;\r\n> \t\t\tdouble slipped_y   = speed_abs_y * duration * slippery_wheels_threshold_;\r\n> \t\t\t\/\/logger_->log_debug(name_, "Wheels are slipping (%f, %f)", slipped_x, slipped_y);\r\n> \t\t\tnew_x = x_ + slipped_x;\r\n> \t\t\tnew_y = y_ + slipped_y;\r\n> \t\t\t\/\/update the offset (otherwise the slippery error would be corrected in the next iteration)\r\n> \t\t\tx_offset_ -= slipped_x;\r\n> \t\t\ty_offset_ -= slipped_y;\r\n> \r\n> \t\t\tlength_driven = sqrt((new_x - x_) * (new_x - x_) + (new_y - y_) * (new_y - y_));\r\n> \t\t}\r\n> \t}\r\n> \r\n> \t\/\/update stored values\r\n> \tx_   = new_x;\r\n> \ty_   = new_y;\r\n> \tori_ = new_ori;\r\n> \tpath_length_ += length_driven;\r\n> \tnew_data_ = true;\r\n> \r\n> \t\/\/publish transform (otherwise the transform can not convert \/base_link to \/odom)\r\n> \tfawkes::Time  now(clock);\r\n> \ttf::Transform t(tf::Quaternion(tf::Vector3(0, 0, 1), ori_), tf::Vector3(x_, y_, 0.0));\r\n> \r\n> \ttf_publisher->send_transform(t, now, cfg_frame_odom_, cfg_frame_base_);\r\n> }\r\n> void\r\n> RobotinoSimThread::on_gyro_msg(ConstVector3dPtr &msg)\r\n> {\r\n> \tMutexLocker lock(loop_mutex);\r\n> \tgyro_buffer_index_new_                         = (gyro_buffer_index_new_ + 1) % gyro_buffer_size_;\r\n> \tgyro_angle_buffer_[gyro_buffer_index_new_]     = msg->z();\r\n> \tgyro_timestamp_buffer_[gyro_buffer_index_new_] = clock->now();\r\n> \tgyro_available_                                = true;\r\n> \tnew_data_                                      = true;\r\n> }\r\n> void\r\n> RobotinoSimThread::on_infrared_puck_sensor_msg(ConstLaserScanStampedPtr &msg)\r\n> {\r\n> \tMutexLocker lock(loop_mutex);\r\n> \t\/\/make sure that the config values for fetch_puck are set right\r\n> \tinfrared_puck_sensor_dist_ = msg->scan().ranges(0);\r\n> \tnew_data_                  = true;\r\n> }\r\n> void\r\n> RobotinoSimThread::on_gripper_laser_right_sensor_msg(ConstFloatPtr &msg)\r\n> {\r\n> \tMutexLocker lock(loop_mutex);\r\n> \r\n> \tif (msg->value() < gripper_laser_threshold_) {\r\n> \t\tanalog_in_right_ = gripper_laser_value_near_;\r\n> \t} else {\r\n> \t\tanalog_in_right_ = gripper_laser_value_far_;\r\n> \t}\r\n> \tnew_data_ = true;\r\n> }\r\n> void\r\n> RobotinoSimThread::on_gripper_laser_left_sensor_msg(ConstFloatPtr &msg)\r\n> {\r\n> \tMutexLocker lock(loop_mutex);\r\n308,404c405,410\r\n<   \/\/read out values + substract offset\r\n<   float new_x = msg->position().x() - x_offset_;\r\n<   float new_y = msg->position().y() - y_offset_;\r\n<   \/\/calculate ori from quaternion\r\n<   float new_ori = tf::get_yaw(tf::Quaternion(msg->orientation().x(), msg->orientation().y()\r\n< \t\t\t\t\t  , msg->orientation().z(), msg->orientation().w()));\r\n<   new_ori -= ori_offset_;\r\n<   \r\n<   \/\/estimate path-length\r\n<   float length_driven = sqrt((new_x-x_) * (new_x-x_) + (new_y-y_) * (new_y-y_));\r\n<   \r\n<   if(slippery_wheels_enabled_)\r\n<   {\r\n<     \/\/simulate slipping wheels when driving against an obstacle\r\n<     fawkes::Time new_time = clock->now();\r\n<     double duration = new_time.in_sec() - last_pos_time_.in_sec();\r\n<     \/\/calculate duration since the velocity was last set to filter slipping while accelerating\r\n<     double velocity_set_duration = new_time.in_sec() - last_vel_set_time_.in_sec();\r\n< \r\n<     last_pos_time_ = new_time;\r\n<     \r\n< \r\n<     double total_speed = sqrt(vx_ * vx_ + vy_ * vy_);\r\n<     if(length_driven < total_speed * duration * slippery_wheels_threshold_ && velocity_set_duration > duration)\r\n<     {\r\n<       double speed_abs_x = vx_ * cos(ori_) - vy_ * sin(ori_);\r\n<       double speed_abs_y = vx_ * sin(ori_) + vy_ * cos(ori_);\r\n<       double slipped_x = speed_abs_x * duration * slippery_wheels_threshold_;\r\n<       double slipped_y = speed_abs_y * duration * slippery_wheels_threshold_;\r\n<       \/\/logger_->log_debug(name_, "Wheels are slipping (%f, %f)", slipped_x, slipped_y);\r\n<       new_x = x_ + slipped_x;\r\n<       new_y = y_ + slipped_y;\r\n<       \/\/update the offset (otherwise the slippery error would be corrected in the next iteration)\r\n<       x_offset_ -= slipped_x;\r\n<       y_offset_ -= slipped_y;\t      \r\n< \r\n<       length_driven = sqrt((new_x-x_) * (new_x-x_) + (new_y-y_) * (new_y-y_));\r\n<     }\r\n<   }\r\n< \r\n<   \/\/update stored values\r\n<   x_ = new_x;\r\n<   y_ = new_y;\r\n<   ori_ = new_ori;\r\n<   path_length_ += length_driven;\r\n<   new_data_ = true;\r\n< \r\n<   \/\/publish transform (otherwise the transform can not convert \/base_link to \/odom)\r\n<   fawkes::Time now(clock);\r\n<   tf::Transform t(tf::Quaternion(tf::Vector3(0,0,1),ori_),\r\n< \t\t  tf::Vector3(x_, y_, 0.0));\r\n< \r\n<   tf_publisher->send_transform(t, now, cfg_frame_odom_, cfg_frame_base_);\r\n< }\r\n< void RobotinoSimThread::on_gyro_msg(ConstVector3dPtr &msg)\r\n< {\r\n<   MutexLocker lock(loop_mutex);\r\n<   gyro_buffer_index_new_ = (gyro_buffer_index_new_ + 1) % gyro_buffer_size_;\r\n<   gyro_angle_buffer_[gyro_buffer_index_new_] = msg->z();\r\n<   gyro_timestamp_buffer_[gyro_buffer_index_new_] = clock->now();\r\n<   gyro_available_ = true;\r\n<   new_data_ = true;\r\n< }\r\n< void RobotinoSimThread::on_infrared_puck_sensor_msg(ConstLaserScanStampedPtr &msg)\r\n< {\r\n<   MutexLocker lock(loop_mutex);\r\n<   \/\/make sure that the config values for fetch_puck are set right\r\n<   infrared_puck_sensor_dist_ = msg->scan().ranges(0);\r\n<   new_data_ = true;\r\n< }\r\n< void RobotinoSimThread::on_gripper_laser_right_sensor_msg(ConstFloatPtr &msg)\r\n< {\r\n<   MutexLocker lock(loop_mutex);\r\n< \r\n<   if(msg->value() < gripper_laser_threshold_)\r\n<   {\r\n<     analog_in_right_ = gripper_laser_value_near_;\r\n<   }\r\n<   else\r\n<   {\r\n<     analog_in_right_ = gripper_laser_value_far_;\r\n<   }\r\n<   new_data_ = true;\r\n< }\r\n< void RobotinoSimThread::on_gripper_laser_left_sensor_msg(ConstFloatPtr &msg)\r\n< {\r\n<   MutexLocker lock(loop_mutex);\r\n< \r\n<   if(msg->value() < gripper_laser_threshold_)\r\n<   {\r\n<     analog_in_left_ = gripper_laser_value_near_;\r\n<   }\r\n<   else\r\n<   {\r\n<     analog_in_left_ = gripper_laser_value_far_;\r\n<   }\r\n<   new_data_ = true;\r\n---\r\n> \tif (msg->value() < gripper_laser_threshold_) {\r\n> \t\tanalog_in_left_ = gripper_laser_value_near_;\r\n> \t} else {\r\n> \t\tanalog_in_left_ = gripper_laser_value_far_;\r\n> \t}\r\n> \tnew_data_ = true;\r\ndiff -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\r\n23,24c23,24\r\n< #ifndef __PLUGINS_GAZSIM_ROBOTINO_THREAD_H_\r\n< #define __PLUGINS_GAZSIM_ROBOTINO_THREAD_H_\r\n---\r\n> #ifndef _PLUGINS_GAZSIM_ROBOTINO_THREAD_H_\r\n> #define _PLUGINS_GAZSIM_ROBOTINO_THREAD_H_\r\n26c26\r\n< #include <list>\r\n---\r\n> #include "..\/msgs\/Float.pb.h"\r\n28c28,29\r\n< #include <core\/threading\/thread.h>\r\n---\r\n> #include <aspect\/blackboard.h>\r\n> #include <aspect\/blocked_timing.h>\r\n32,34d32\r\n< #include <aspect\/blackboard.h>\r\n< #include <aspect\/blocked_timing.h>\r\n< #include <plugins\/gazebo\/aspect\/gazebo.h>\r\n36c34,37\r\n< #include "..\/msgs\/Float.pb.h"\r\n---\r\n> #include <core\/threading\/thread.h>\r\n> #include <plugins\/gazebo\/aspect\/gazebo.h>\r\n> \r\n> #include <list>\r\n39d39\r\n< #include <gazebo\/transport\/TransportTypes.hh>\r\n40a41\r\n> #include <gazebo\/transport\/TransportTypes.hh>\r\n43d43\r\n< \r\n47,62c47,61\r\n<   class BatteryInterface;\r\n<   class IMUInterface;\r\n<   class MotorInterface;\r\n<   class RobotinoSensorInterface;\r\n<   class SwitchInterface;\r\n< }\r\n< \r\n< class RobotinoSimThread\r\n< : public fawkes::Thread,\r\n<   public fawkes::ClockAspect,\r\n<   public fawkes::LoggingAspect,\r\n<   public fawkes::ConfigurableAspect,\r\n<   public fawkes::BlackBoardAspect,\r\n<   public fawkes::BlockedTimingAspect,\r\n<   public fawkes::TransformAspect,\r\n<   public fawkes::GazeboAspect\r\n---\r\n> class BatteryInterface;\r\n> class IMUInterface;\r\n> class MotorInterface;\r\n> class RobotinoSensorInterface;\r\n> class SwitchInterface;\r\n> } \/\/ namespace fawkes\r\n> \r\n> class RobotinoSimThread : public fawkes::Thread,\r\n>                           public fawkes::ClockAspect,\r\n>                           public fawkes::LoggingAspect,\r\n>                           public fawkes::ConfigurableAspect,\r\n>                           public fawkes::BlackBoardAspect,\r\n>                           public fawkes::BlockedTimingAspect,\r\n>                           public fawkes::TransformAspect,\r\n>                           public fawkes::GazeboAspect\r\n64,65c63,64\r\n<  public:\r\n<   RobotinoSimThread();\r\n---\r\n> public:\r\n> \tRobotinoSimThread();\r\n67,151c66,152\r\n<   virtual void init();\r\n<   virtual void loop();\r\n<   virtual void finalize();\r\n<  private:\r\n<   \/\/Publisher to send messages to gazebo\r\n<   gazebo::transport::PublisherPtr string_pub_;\r\n<   gazebo::transport::PublisherPtr motor_move_pub_;\r\n< \r\n<   \/\/Suscribers to recieve messages from gazebo\r\n<   gazebo::transport::SubscriberPtr gyro_sub_;\r\n<   gazebo::transport::SubscriberPtr infrared_puck_sensor_sub_;\r\n<   gazebo::transport::SubscriberPtr gripper_laser_left_sensor_sub_;\r\n<   gazebo::transport::SubscriberPtr gripper_laser_right_sensor_sub_;\r\n<   gazebo::transport::SubscriberPtr pos_sub_;\r\n< \r\n<   \/\/Handler functions for incoming messages\r\n<   void on_gyro_msg(ConstVector3dPtr &msg);\r\n<   void on_infrared_puck_sensor_msg(ConstLaserScanStampedPtr &msg);\r\n<   void on_gripper_laser_left_sensor_msg(ConstFloatPtr &msg);\r\n<   void on_gripper_laser_right_sensor_msg(ConstFloatPtr &msg);\r\n<   void on_pos_msg(ConstPosePtr &msg);\r\n< \r\n<   \/\/provided interfaces\r\n<   fawkes::RobotinoSensorInterface *sens_if_;\r\n<   fawkes::MotorInterface          *motor_if_;\r\n<   fawkes::SwitchInterface         *switch_if_;\r\n<   fawkes::IMUInterface            *imu_if_;\r\n< \r\n<   \/\/config values\r\n<   std::string cfg_frame_odom_;\r\n<   std::string cfg_frame_base_;\r\n<   double gripper_laser_threshold_;\r\n<   double gripper_laser_value_far_;\r\n<   double gripper_laser_value_near_;\r\n<   bool slippery_wheels_enabled_;\r\n<   double slippery_wheels_threshold_;\r\n<   double moving_speed_factor_;\r\n<   double rotation_speed_factor_;\r\n<   bool have_gripper_sensors_;\r\n<   int gripper_laser_left_pos_;\r\n<   int gripper_laser_right_pos_;\r\n<   int infrared_sensor_index_;\r\n< \r\n<   \/\/Helper variables for motor:\r\n< \r\n<   \/\/current motorMovements\r\n<   float vx_;\r\n<   float vy_;\r\n<   float vomega_;\r\n<   float des_vx_;\r\n<   float des_vy_;\r\n<   float des_vomega_;\r\n<   \/\/last received odom position\r\n<   float x_;\r\n<   float y_;\r\n<   float ori_;\r\n<   float path_length_;\r\n<  \r\n<   \/\/RobotinoSensorInterface values (stored here to write the interfaces only in the loop)\r\n<   bool gyro_available_;\r\n<   int gyro_buffer_size_;\r\n<   int gyro_buffer_index_new_;\r\n<   int gyro_buffer_index_delayed_;\r\n<   fawkes::Time *gyro_timestamp_buffer_;\r\n<   float *gyro_angle_buffer_;\r\n<   float gyro_delay_;\r\n<   float infrared_puck_sensor_dist_;\r\n<   float analog_in_left_;\r\n<   float analog_in_right_;\r\n< \r\n<   \/\/are there new values to write in the interfaces?\r\n<   bool new_data_;\r\n< \r\n<   fawkes::Time last_pos_time_;\r\n<   fawkes::Time last_vel_set_time_;\r\n< \r\n<   \/\/Odometry offset\r\n<   float x_offset_;\r\n<   float y_offset_;\r\n<   float ori_offset_;\r\n< \r\n<   \/\/Helper functions:\r\n<   void process_motor_messages();\r\n<   void send_transroot(double vx, double vy, double omega);\r\n<   bool vel_changed(float before, float after, float relativeThreashold);  \r\n---\r\n> \tvirtual void init();\r\n> \tvirtual void loop();\r\n> \tvirtual void finalize();\r\n> \r\n> private:\r\n> \t\/\/Publisher to send messages to gazebo\r\n> \tgazebo::transport::PublisherPtr string_pub_;\r\n> \tgazebo::transport::PublisherPtr motor_move_pub_;\r\n> \r\n> \t\/\/Suscribers to recieve messages from gazebo\r\n> \tgazebo::transport::SubscriberPtr gyro_sub_;\r\n> \tgazebo::transport::SubscriberPtr infrared_puck_sensor_sub_;\r\n> \tgazebo::transport::SubscriberPtr gripper_laser_left_sensor_sub_;\r\n> \tgazebo::transport::SubscriberPtr gripper_laser_right_sensor_sub_;\r\n> \tgazebo::transport::SubscriberPtr pos_sub_;\r\n> \r\n> \t\/\/Handler functions for incoming messages\r\n> \tvoid on_gyro_msg(ConstVector3dPtr &msg);\r\n> \tvoid on_infrared_puck_sensor_msg(ConstLaserScanStampedPtr &msg);\r\n> \tvoid on_gripper_laser_left_sensor_msg(ConstFloatPtr &msg);\r\n> \tvoid on_gripper_laser_right_sensor_msg(ConstFloatPtr &msg);\r\n> \tvoid on_pos_msg(ConstPosePtr &msg);\r\n> \r\n> \t\/\/provided interfaces\r\n> \tfawkes::RobotinoSensorInterface *sens_if_;\r\n> \tfawkes::MotorInterface *         motor_if_;\r\n> \tfawkes::SwitchInterface *        switch_if_;\r\n> \tfawkes::IMUInterface *           imu_if_;\r\n> \r\n> \t\/\/config values\r\n> \tstd::string cfg_frame_odom_;\r\n> \tstd::string cfg_frame_base_;\r\n> \tstd::string cfg_frame_imu_;\r\n> \tdouble      gripper_laser_threshold_;\r\n> \tdouble      gripper_laser_value_far_;\r\n> \tdouble      gripper_laser_value_near_;\r\n> \tbool        slippery_wheels_enabled_;\r\n> \tdouble      slippery_wheels_threshold_;\r\n> \tdouble      moving_speed_factor_;\r\n> \tdouble      rotation_speed_factor_;\r\n> \tbool        have_gripper_sensors_;\r\n> \tint         gripper_laser_left_pos_;\r\n> \tint         gripper_laser_right_pos_;\r\n> \tint         infrared_sensor_index_;\r\n> \r\n> \t\/\/Helper variables for motor:\r\n> \r\n> \t\/\/current motorMovements\r\n> \tfloat vx_;\r\n> \tfloat vy_;\r\n> \tfloat vomega_;\r\n> \tfloat des_vx_;\r\n> \tfloat des_vy_;\r\n> \tfloat des_vomega_;\r\n> \t\/\/last received odom position\r\n> \tfloat x_;\r\n> \tfloat y_;\r\n> \tfloat ori_;\r\n> \tfloat path_length_;\r\n> \r\n> \t\/\/RobotinoSensorInterface values (stored here to write the interfaces only in the loop)\r\n> \tbool          gyro_available_;\r\n> \tint           gyro_buffer_size_;\r\n> \tint           gyro_buffer_index_new_;\r\n> \tint           gyro_buffer_index_delayed_;\r\n> \tfawkes::Time *gyro_timestamp_buffer_;\r\n> \tfloat *       gyro_angle_buffer_;\r\n> \tfloat         gyro_delay_;\r\n> \tfloat         infrared_puck_sensor_dist_;\r\n> \tfloat         analog_in_left_;\r\n> \tfloat         analog_in_right_;\r\n> \r\n> \t\/\/are there new values to write in the interfaces?\r\n> \tbool new_data_;\r\n> \r\n> \tfawkes::Time last_pos_time_;\r\n> \tfawkes::Time last_vel_set_time_;\r\n> \r\n> \t\/\/Odometry offset\r\n> \tfloat x_offset_;\r\n> \tfloat y_offset_;\r\n> \tfloat ori_offset_;\r\n> \r\n> \t\/\/Helper functions:\r\n> \tvoid process_motor_messages();\r\n> \tvoid send_transroot(double vx, double vy, double omega);\r\n> \tbool vel_changed(float before, float after, float relativeThreashold);\r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/gazsim-robotino\/Makefile fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/gazsim-robotino\/Makefile\r\n27,31c27,31\r\n< LIBS_gazsim_robotino = fawkescore fawkesutils fawkesaspects fvutils \\\r\n< \t\tfawkestf fawkesinterface fawkesblackboard \\\r\n< \t\tBatteryInterface RobotinoSensorInterface MotorInterface \\\r\n< \t\tPosition3DInterface IMUInterface SwitchInterface \\\r\n< \t\tfawkesgazeboaspect gazsim_msgs \\\r\n---\r\n> LIBS_gazsim_robotino = m fawkescore fawkesutils fawkesaspects fvutils \\\r\n>                        fawkestf fawkesinterface fawkesblackboard \\\r\n>                        BatteryInterface RobotinoSensorInterface MotorInterface \\\r\n>                        Position3DInterface IMUInterface SwitchInterface \\\r\n>                        fawkesgazeboaspect gazsim_msgs \\\r\n35a36\r\n> PLUGINS_all = $(PLUGINDIR)\/gazsim-robotino.so\r\n43c44\r\n<   PLUGINS_all = $(PLUGINDIR)\/gazsim-robotino.so\r\n---\r\n>   PLUGINS_build = $(PLUGINS_all)\r\ndiff -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\r\n23a24\r\n> \r\n33,34c34,35\r\n<  public:\r\n<   \/** Constructor.\r\n---\r\n> public:\r\n> \t\/** Constructor.\r\n37,40c38,41\r\n<   GazsimTimesourcePlugin(Configuration *config) : Plugin(config)\r\n<   {\r\n<     thread_list.push_back(new GazsimTimesourceThread());\r\n<   }\r\n---\r\n> \texplicit GazsimTimesourcePlugin(Configuration *config) : Plugin(config)\r\n> \t{\r\n> \t\tthread_list.push_back(new GazsimTimesourceThread());\r\n> \t}\r\n42d42\r\n< \r\ndiff -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\r\n31c31\r\n< GazsimTimesource::GazsimTimesource(Clock* clock)\r\n---\r\n> GazsimTimesource::GazsimTimesource(Clock *clock)\r\n33c33\r\n<   clock_ = clock;\r\n---\r\n> \tclock_ = clock;\r\n35,38c35,38\r\n<   last_sim_time_ = get_system_time();\r\n<   last_real_time_factor_ = 1.0;\r\n<   clock_->get_systime(&last_sys_recv_time_);\r\n<   \/\/registration will be done by plugin\r\n---\r\n> \tlast_sim_time_         = get_system_time();\r\n> \tlast_real_time_factor_ = 1.0;\r\n> \tclock_->get_systime(&last_sys_recv_time_);\r\n> \t\/\/registration will be done by plugin\r\n43d42\r\n< \r\n52c51\r\n< GazsimTimesource::get_time(timeval* tv) const\r\n---\r\n> GazsimTimesource::get_time(timeval *tv) const\r\n54,62c53,62\r\n<   \/\/I do not use the Time - operator here because this would recursively call get_time\r\n<   timeval now = get_system_time();\r\n<   timeval interval = subtract(now, last_sys_recv_time_);  \r\n< \r\n<   \/\/doing this: timeval estimated_sim_interval = interval * last_real_time_factor_;\r\n<   timeval estimated_sim_interval;\r\n<   estimated_sim_interval.tv_usec = last_real_time_factor_ * (interval.tv_sec * 1000000  + interval.tv_usec);\r\n<   estimated_sim_interval.tv_sec = estimated_sim_interval.tv_usec \/ 1000000;\r\n<   estimated_sim_interval.tv_usec -= estimated_sim_interval.tv_sec * 1000000;\r\n---\r\n> \t\/\/I do not use the Time - operator here because this would recursively call get_time\r\n> \ttimeval now      = get_system_time();\r\n> \ttimeval interval = subtract(now, last_sys_recv_time_);\r\n> \r\n> \t\/\/doing this: timeval estimated_sim_interval = interval * last_real_time_factor_;\r\n> \ttimeval estimated_sim_interval;\r\n> \testimated_sim_interval.tv_usec =\r\n> \t  last_real_time_factor_ * (interval.tv_sec * 1000000 + interval.tv_usec);\r\n> \testimated_sim_interval.tv_sec = estimated_sim_interval.tv_usec \/ 1000000;\r\n> \testimated_sim_interval.tv_usec -= estimated_sim_interval.tv_sec * 1000000;\r\n64c64\r\n<   timeval estimated_sim_now = add(last_sim_time_, estimated_sim_interval);\r\n---\r\n> \ttimeval estimated_sim_now = add(last_sim_time_, estimated_sim_interval);\r\n66,67c66,67\r\n<   \/\/return\r\n<   *tv =  estimated_sim_now;\r\n---\r\n> \t\/\/return\r\n> \t*tv = estimated_sim_now;\r\n71c71\r\n< GazsimTimesource::conv_to_realtime(const timeval* tv) const\r\n---\r\n> GazsimTimesource::conv_to_realtime(const timeval *tv) const\r\n73c73\r\n<   timeval interval = subtract(*tv, last_sim_time_);\r\n---\r\n> \ttimeval interval = subtract(*tv, last_sim_time_);\r\n75,79c75,80\r\n<   \/\/doing this: timeval est_real_interval = interval \/ last_real_time_factor_;\r\n<   timeval est_real_interval;\r\n<   est_real_interval.tv_usec = (interval.tv_sec * 1000000  + interval.tv_usec) \/ last_real_time_factor_;\r\n<   est_real_interval.tv_sec = est_real_interval.tv_usec \/ 1000000;\r\n<   est_real_interval.tv_usec -= est_real_interval.tv_sec * 1000000;\r\n---\r\n> \t\/\/doing this: timeval est_real_interval = interval \/ last_real_time_factor_;\r\n> \ttimeval est_real_interval;\r\n> \test_real_interval.tv_usec =\r\n> \t  (interval.tv_sec * 1000000 + interval.tv_usec) \/ last_real_time_factor_;\r\n> \test_real_interval.tv_sec = est_real_interval.tv_usec \/ 1000000;\r\n> \test_real_interval.tv_usec -= est_real_interval.tv_sec * 1000000;\r\n81,82c82,83\r\n<   timeval result = add(last_sys_recv_time_, est_real_interval);\r\n<   return result;\r\n---\r\n> \ttimeval result = add(last_sys_recv_time_, est_real_interval);\r\n> \treturn result;\r\n86c87\r\n< GazsimTimesource::conv_native_to_exttime(const timeval* tv) const\r\n---\r\n> GazsimTimesource::conv_native_to_exttime(const timeval *tv) const\r\n88,95c89,96\r\n<   timeval t_offset    = subtract(*tv, last_native_sim_time_);\r\n<   double  offset      = t_offset.tv_sec + t_offset.tv_usec \/ 1000000.f;\r\n<   long    offset_sec  = ::ceil(offset);\r\n<   long    offset_usec = ::round(offset - offset_sec) * 1000000;\r\n< \r\n<   timeval rv;\r\n<   rv.tv_sec  = last_sim_time_.tv_sec  + offset_sec;\r\n<   rv.tv_usec = last_sim_time_.tv_usec + offset_usec;\r\n---\r\n> \ttimeval t_offset    = subtract(*tv, last_native_sim_time_);\r\n> \tdouble  offset      = t_offset.tv_sec + t_offset.tv_usec \/ 1000000.f;\r\n> \tlong    offset_sec  = ::ceil(offset);\r\n> \tlong    offset_usec = ::round(offset - offset_sec) * 1000000;\r\n> \r\n> \ttimeval rv;\r\n> \trv.tv_sec  = last_sim_time_.tv_sec + offset_sec;\r\n> \trv.tv_usec = last_sim_time_.tv_usec + offset_usec;\r\n97c98\r\n<   return rv;\r\n---\r\n> \treturn rv;\r\n106,111c107,112\r\n<   \/\/we do not want to correct time back\r\n<   get_time(&last_sim_time_);\r\n<   last_real_time_factor_ = msg->real_time_factor();\r\n<   clock_->get_systime(&last_sys_recv_time_);\r\n<   last_native_sim_time_.tv_sec  = msg->sim_time_sec();\r\n<   last_native_sim_time_.tv_usec = msg->sim_time_nsec() \/ 1000;\r\n---\r\n> \t\/\/we do not want to correct time back\r\n> \tget_time(&last_sim_time_);\r\n> \tlast_real_time_factor_ = msg->real_time_factor();\r\n> \tclock_->get_systime(&last_sys_recv_time_);\r\n> \tlast_native_sim_time_.tv_sec  = msg->sim_time_sec();\r\n> \tlast_native_sim_time_.tv_usec = msg->sim_time_nsec() \/ 1000;\r\n117,119c118,120\r\n<   timeval now_timeval;\r\n<   gettimeofday(&now_timeval,NULL);\r\n<   return now_timeval;\r\n---\r\n> \ttimeval now_timeval;\r\n> \tgettimeofday(&now_timeval, NULL);\r\n> \treturn now_timeval;\r\n125,133c126,133\r\n<   timeval res;\r\n<   res.tv_sec = a.tv_sec + b.tv_sec;\r\n<   res.tv_usec = a.tv_usec + b.tv_usec;\r\n<   if(res.tv_usec > 1000000)\r\n<   {\r\n<     res.tv_usec -= 1000000;\r\n<     res.tv_sec++;\r\n<   }\r\n<   return res;\r\n---\r\n> \ttimeval res;\r\n> \tres.tv_sec  = a.tv_sec + b.tv_sec;\r\n> \tres.tv_usec = a.tv_usec + b.tv_usec;\r\n> \tif (res.tv_usec > 1000000) {\r\n> \t\tres.tv_usec -= 1000000;\r\n> \t\tres.tv_sec++;\r\n> \t}\r\n> \treturn res;\r\n139,150c139,147\r\n<   timeval res;\r\n<   res.tv_sec = a.tv_sec - b.tv_sec;\r\n<   if(a.tv_usec >= b.tv_usec)\r\n<   {\r\n<     res.tv_usec = a.tv_usec - b.tv_usec;\r\n<   }\r\n<   else\r\n<   {\r\n<     res.tv_usec = 1000000 + a.tv_usec - b.tv_usec;\r\n<     res.tv_sec--;\r\n<   }\r\n<   return res;\r\n---\r\n> \ttimeval res;\r\n> \tres.tv_sec = a.tv_sec - b.tv_sec;\r\n> \tif (a.tv_usec >= b.tv_usec) {\r\n> \t\tres.tv_usec = a.tv_usec - b.tv_usec;\r\n> \t} else {\r\n> \t\tres.tv_usec = 1000000 + a.tv_usec - b.tv_usec;\r\n> \t\tres.tv_sec--;\r\n> \t}\r\n> \treturn res;\r\ndiff -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\r\n23,26d22\r\n< #include <boost\/asio.hpp>\r\n< #include <utils\/time\/timesource.h>\r\n< #include <utils\/time\/clock.h>\r\n< \r\n29,30c25,26\r\n< #ifndef __GAZEBO_TIMESOURCE_H_\r\n< #define __GAZEBO_TIMESOURCE_H_\r\n---\r\n> #include <utils\/time\/clock.h>\r\n> #include <utils\/time\/timesource.h>\r\n31a28,31\r\n> #include <boost\/asio.hpp>\r\n> \r\n> #ifndef _GAZEBO_TIMESOURCE_H_\r\n> #\tdefine _GAZEBO_TIMESOURCE_H_\r\n35,37c35,36\r\n< namespace fawkes\r\n< {\r\n<   \/** @class GazsimTimesource\r\n---\r\n> namespace fawkes {\r\n> \/** @class GazsimTimesource\r\n41,70c40,68\r\n<   class GazsimTimesource : public TimeSource\r\n<   {\r\n<   public:\r\n<     \/\/Constructor\r\n<     GazsimTimesource(Clock* clock);\r\n<     \/\/\/Destructor\r\n<    ~GazsimTimesource();\r\n< \r\n<     virtual void get_time(timeval* tv) const;\r\n<     virtual timeval conv_to_realtime(const timeval* tv) const;\r\n<     virtual timeval conv_native_to_exttime(const timeval* tv) const;\r\n< \r\n<     \/\/\/ store data from gazebo time message\r\n<     void on_time_sync_msg(ConstSimTimePtr &msg);\r\n< \r\n<   private:\r\n<     timeval get_system_time() const;\r\n<     timeval add(timeval a, timeval b) const;\r\n<     timeval subtract(timeval a, timeval b) const;\r\n< \r\n<   private:\r\n<     Clock* clock_;\r\n< \r\n<     \/\/from last msg all in sec\r\n<     timeval last_sim_time_;\r\n<     timeval last_sys_recv_time_;\r\n<     double  last_real_time_factor_;\r\n<     timeval last_native_sim_time_;\r\n< \r\n<   };\r\n---\r\n> class GazsimTimesource : public TimeSource\r\n> {\r\n> public:\r\n> \t\/\/Constructor\r\n> \tGazsimTimesource(Clock *clock);\r\n> \t\/\/\/Destructor\r\n> \t~GazsimTimesource();\r\n> \r\n> \tvirtual void    get_time(timeval *tv) const;\r\n> \tvirtual timeval conv_to_realtime(const timeval *tv) const;\r\n> \tvirtual timeval conv_native_to_exttime(const timeval *tv) const;\r\n> \r\n> \t\/\/\/ store data from gazebo time message\r\n> \tvoid on_time_sync_msg(ConstSimTimePtr &msg);\r\n> \r\n> private:\r\n> \ttimeval get_system_time() const;\r\n> \ttimeval add(timeval a, timeval b) const;\r\n> \ttimeval subtract(timeval a, timeval b) const;\r\n> \r\n> private:\r\n> \tClock *clock_;\r\n> \r\n> \t\/\/from last msg all in sec\r\n> \ttimeval last_sim_time_;\r\n> \ttimeval last_sys_recv_time_;\r\n> \tdouble  last_real_time_factor_;\r\n> \ttimeval last_native_sim_time_;\r\n> };\r\n72c70\r\n< }\r\n---\r\n> } \/\/ namespace fawkes\r\ndiff -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\r\n33,34c33,34\r\n<   : Thread("GazsimTimesourceThread", Thread::OPMODE_WAITFORWAKEUP),\r\n<     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE)\r\n---\r\n> : Thread("GazsimTimesourceThread", Thread::OPMODE_WAITFORWAKEUP),\r\n>   BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE)\r\n42,43c42,43\r\n< \r\n< void GazsimTimesourceThread::init()\r\n---\r\n> void\r\n> GazsimTimesourceThread::init()\r\n45c45\r\n<   logger->log_info(name(), "GazsimTimesource initializing");\r\n---\r\n> \tlogger->log_info(name(), "GazsimTimesource initializing");\r\n47,48c47,50\r\n<   \/\/Create Subscriber\r\n<   time_sync_sub_ = gazebo_world_node->Subscribe(config->get_string("\/gazsim\/topics\/time"), &GazsimTimesourceThread::on_time_sync_msg, this);\r\n---\r\n> \t\/\/Create Subscriber\r\n> \ttime_sync_sub_ = gazebo_world_node->Subscribe(config->get_string("\/gazsim\/topics\/time"),\r\n> \t                                              &GazsimTimesourceThread::on_time_sync_msg,\r\n> \t                                              this);\r\n50,51c52,53\r\n<   \/\/Create Time Source\r\n<   time_source_ = new GazsimTimesource(clock);\r\n---\r\n> \t\/\/Create Time Source\r\n> \ttime_source_ = new GazsimTimesource(clock);\r\n53,54c55,56\r\n<   \/\/register timesource and make it default\r\n<   clock->register_ext_timesource(time_source_, true);\r\n---\r\n> \t\/\/register timesource and make it default\r\n> \tclock->register_ext_timesource(time_source_, true);\r\n57,58c59,60\r\n< \r\n< void GazsimTimesourceThread::finalize()\r\n---\r\n> void\r\n> GazsimTimesourceThread::finalize()\r\n60,62c62,64\r\n<   \/\/remove time source\r\n<   clock->remove_ext_timesource(time_source_);\r\n<   delete time_source_;\r\n---\r\n> \t\/\/remove time source\r\n> \tclock->remove_ext_timesource(time_source_);\r\n> \tdelete time_source_;\r\n65,66c67,68\r\n< \r\n< void GazsimTimesourceThread::loop()\r\n---\r\n> void\r\n> GazsimTimesourceThread::loop()\r\n68c70\r\n<   \/\/nothing interesting\r\n---\r\n> \t\/\/nothing interesting\r\n71c73,74\r\n< void GazsimTimesourceThread::on_time_sync_msg(ConstSimTimePtr &msg)\r\n---\r\n> void\r\n> GazsimTimesourceThread::on_time_sync_msg(ConstSimTimePtr &msg)\r\n73,76c76,79\r\n<   \/\/ logger->log_info(name(), "Got Simulation Time");\r\n<   \r\n<   \/\/provide time source with newest message\r\n<   time_source_->on_time_sync_msg(msg);\r\n---\r\n> \t\/\/ logger->log_info(name(), "Got Simulation Time");\r\n> \r\n> \t\/\/provide time source with newest message\r\n> \ttime_source_->on_time_sync_msg(msg);\r\ndiff -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\r\n23,24c23,24\r\n< #ifndef __PLUGINS_GAZSIM__TIMESOURCE_THREAD_H_\r\n< #define __PLUGINS_GAZSIM__TIMESOURCE_THREAD_H_\r\n---\r\n> #ifndef _PLUGINS_GAZSIMTIMESOURCE_THREAD_H__\r\n> #define _PLUGINS_GAZSIMTIMESOURCE_THREAD_H__\r\n26c26,29\r\n< #include <core\/threading\/thread.h>\r\n---\r\n> #include "..\/msgs\/SimTime.pb.h"\r\n> #include "gazsim_timesource_source.h"\r\n> \r\n> #include <aspect\/blocked_timing.h>\r\n28d30\r\n< #include <aspect\/logging.h>\r\n30,31c32,33\r\n< #include <aspect\/blocked_timing.h>\r\n< #include <boost\/asio.hpp>\r\n---\r\n> #include <aspect\/logging.h>\r\n> #include <core\/threading\/thread.h>\r\n33d34\r\n< #include <gazebo\/physics\/physics.hh>\r\n36,37c37,38\r\n< #include "gazsim_timesource_source.h"\r\n< #include "..\/msgs\/SimTime.pb.h"\r\n---\r\n> #include <boost\/asio.hpp>\r\n> #include <gazebo\/physics\/physics.hh>\r\n41,47c42,47\r\n< class GazsimTimesourceThread\r\n< : public fawkes::Thread,\r\n<   public fawkes::ClockAspect,\r\n<   public fawkes::BlockedTimingAspect,\r\n<   public fawkes::ConfigurableAspect,\r\n<   public fawkes::LoggingAspect,\r\n<   public fawkes::GazeboAspect\r\n---\r\n> class GazsimTimesourceThread : public fawkes::Thread,\r\n>                                public fawkes::ClockAspect,\r\n>                                public fawkes::BlockedTimingAspect,\r\n>                                public fawkes::ConfigurableAspect,\r\n>                                public fawkes::LoggingAspect,\r\n>                                public fawkes::GazeboAspect\r\n49,62c49,59\r\n<  public:\r\n<   GazsimTimesourceThread();\r\n<   ~GazsimTimesourceThread();\r\n< \r\n<   virtual void init();\r\n<   virtual void loop();\r\n<   virtual void finalize();\r\n< \r\n<  private:\r\n<   \/\/Timesource to give the fawkes clock\r\n<   fawkes::GazsimTimesource* time_source_;\r\n< \r\n<   \/\/subscriber to get time msgs from\r\n<   gazebo::transport::SubscriberPtr time_sync_sub_;\r\n---\r\n> public:\r\n> \tGazsimTimesourceThread();\r\n> \t~GazsimTimesourceThread();\r\n> \r\n> \tvirtual void init();\r\n> \tvirtual void loop();\r\n> \tvirtual void finalize();\r\n> \r\n> private:\r\n> \t\/\/Timesource to give the fawkes clock\r\n> \tfawkes::GazsimTimesource *time_source_;\r\n64,65c61,62\r\n<   \/\/handler\r\n<   void on_time_sync_msg(ConstSimTimePtr &msg);\r\n---\r\n> \t\/\/subscriber to get time msgs from\r\n> \tgazebo::transport::SubscriberPtr time_sync_sub_;\r\n66a64,65\r\n> \t\/\/handler\r\n> \tvoid on_time_sync_msg(ConstSimTimePtr &msg);\r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/gazsim-timesource\/Makefile fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/gazsim-timesource\/Makefile\r\n25d24\r\n< \r\n31c30,31\r\n< OBJS_all = $(OBJS_gazsim_timesource)\r\n---\r\n> OBJS_all    = $(OBJS_gazsim_timesource)\r\n> PLUGINS_all = $(PLUGINDIR)\/gazsim-timesource.so\r\n38,39c38,39\r\n< \t     $(call boost-libs-cflags,$(REQ_BOOST_LIBS)) \\\r\n< \t     $(CFLAGS_GAZEBO)\r\n---\r\n>              $(call boost-libs-cflags,$(REQ_BOOST_LIBS)) \\\r\n>              $(CFLAGS_GAZEBO)\r\n41,42c41,42\r\n< \t     $(call boost-libs-ldflags,$(REQ_BOOST_LIBS)) \\\r\n< \t     $(LDFLAGS_GAZEBO)\r\n---\r\n>              $(call boost-libs-ldflags,$(REQ_BOOST_LIBS)) \\\r\n>              $(LDFLAGS_GAZEBO)\r\n44c44\r\n<   PLUGINS_all = $(PLUGINDIR)\/gazsim-timesource.so\r\n---\r\n>   PLUGINS_build = $(PLUGINS_all)\r\ndiff -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\r\n21,22d20\r\n< #include <core\/plugin.h>\r\n< \r\n24a23,24\r\n> #include <core\/plugin.h>\r\n> \r\n34,36c34,35\r\n<  public:\r\n< \r\n<   \/** Constructor.\r\n---\r\n> public:\r\n> \t\/** Constructor.\r\n39,43c38,41\r\n<   GazsimVisLocalizationPlugin(Configuration *config)\r\n<     : Plugin(config)\r\n<   {\r\n<     thread_list.push_back(new VisLocalizationThread());\r\n<   }\r\n---\r\n> \texplicit GazsimVisLocalizationPlugin(Configuration *config) : Plugin(config)\r\n> \t{\r\n> \t\tthread_list.push_back(new VisLocalizationThread());\r\n> \t}\r\ndiff -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\r\n22a23,24\r\n> #include <aspect\/logging.h>\r\n> #include <interfaces\/Position3DInterface.h>\r\n24,25d25\r\n< #include <stdio.h>\r\n< #include <cmath>\r\n28,30c28\r\n< #include <interfaces\/Position3DInterface.h>\r\n< \r\n< #include <gazebo\/transport\/Node.hh>\r\n---\r\n> #include <cmath>\r\n31a30\r\n> #include <gazebo\/transport\/Node.hh>\r\n33c32\r\n< #include <aspect\/logging.h>\r\n---\r\n> #include <stdio.h>\r\n45,46c44,45\r\n<   : Thread("VisLocalizationThread", Thread::OPMODE_WAITFORWAKEUP),\r\n<     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE)\r\n---\r\n> : Thread("VisLocalizationThread", Thread::OPMODE_WAITFORWAKEUP),\r\n>   BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE)\r\n50c49,50\r\n< void VisLocalizationThread::init()\r\n---\r\n> void\r\n> VisLocalizationThread::init()\r\n52c52\r\n<   logger->log_debug(name(), "Initializing Visualization of the Localization");\r\n---\r\n> \tlogger->log_debug(name(), "Initializing Visualization of the Localization");\r\n54,63c54,63\r\n<   \/\/read config values\r\n<   update_rate_ = config->get_float("\/gazsim\/visualization\/localization\/update-rate");\r\n<   robot_name_ = config->get_string("\/gazsim\/robot-name");\r\n<   label_script_name_ = config->get_string("\/gazsim\/visualization\/label-script-name");\r\n<   arrow_script_name_ = config->get_string("\/gazsim\/visualization\/label-arrow-name");\r\n<   location_scripts_ = config->get_string("\/gazsim\/visualization\/location-scripts");\r\n<   location_textures_ = config->get_string("\/gazsim\/visualization\/location-textures");\r\n<   parent_name_ = config->get_string("\/gazsim\/visualization\/localization\/parent-name");\r\n<   label_size_ = config->get_float("\/gazsim\/visualization\/localization\/label-size");\r\n<   label_height_ = config->get_float("\/gazsim\/visualization\/localization\/label-height");\r\n---\r\n> \t\/\/read config values\r\n> \tupdate_rate_       = config->get_float("\/gazsim\/visualization\/localization\/update-rate");\r\n> \trobot_name_        = config->get_string("\/gazsim\/robot-name");\r\n> \tlabel_script_name_ = config->get_string("\/gazsim\/visualization\/label-script-name");\r\n> \tarrow_script_name_ = config->get_string("\/gazsim\/visualization\/label-arrow-name");\r\n> \tlocation_scripts_  = config->get_string("\/gazsim\/visualization\/location-scripts");\r\n> \tlocation_textures_ = config->get_string("\/gazsim\/visualization\/location-textures");\r\n> \tparent_name_       = config->get_string("\/gazsim\/visualization\/localization\/parent-name");\r\n> \tlabel_size_        = config->get_float("\/gazsim\/visualization\/localization\/label-size");\r\n> \tlabel_height_      = config->get_float("\/gazsim\/visualization\/localization\/label-height");\r\n65c65\r\n<   last_update_time_ = clock->now().in_sec();\r\n---\r\n> \tlast_update_time_ = clock->now().in_sec();\r\n67,68c67,68\r\n<   \/\/open interface\r\n<   pose_if_ = blackboard->open_for_reading<Position3DInterface>("Pose");\r\n---\r\n> \t\/\/open interface\r\n> \tpose_if_ = blackboard->open_for_reading<Position3DInterface>("Pose");\r\n70,71c70,71\r\n<   \/\/create publisher\r\n<   visual_publisher_ = gazebo_world_node->Advertise<gazebo::msgs::Visual>("~\/visual", 5);\r\n---\r\n> \t\/\/create publisher\r\n> \tvisual_publisher_ = gazebo_world_node->Advertise<gazebo::msgs::Visual>("~\/visual", 5);\r\n74c74,75\r\n< void VisLocalizationThread::finalize()\r\n---\r\n> void\r\n> VisLocalizationThread::finalize()\r\n76c77\r\n<   blackboard->close(pose_if_);\r\n---\r\n> \tblackboard->close(pose_if_);\r\n79c80,81\r\n< void VisLocalizationThread::loop()\r\n---\r\n> void\r\n> VisLocalizationThread::loop()\r\n81,105c83,105\r\n<   \/\/visualize the estimated position of the robot every few seconds\r\n<   fawkes::Time new_time = clock->now();\r\n<   double time_elapsed = new_time.in_sec() - last_update_time_.in_sec();\r\n<   if(time_elapsed > 1 \/ update_rate_)\r\n<   {\r\n<     last_update_time_ = new_time;\r\n< \r\n<     \/\/read pose\r\n<     pose_if_->read();\r\n<     double x = pose_if_->translation(0);\r\n<     double y = pose_if_->translation(1);\r\n<     \/\/calculate ori from quaternion in interface\r\n<     double* quat = pose_if_->rotation();\r\n<     double ori = tf::get_yaw(tf::Quaternion(quat[0], quat[1], quat[2], quat[3]));\r\n<     if(std::isnan(ori))\r\n<     {\r\n<       ori = 0.0;\r\n<     }\r\n< \r\n<     \/\/create label with number\r\n<     msgs::Visual msg_number;\r\n<     msg_number.set_name((robot_name_ + "-localization-label").c_str());\r\n<     msg_number.set_parent_name(parent_name_.c_str());\r\n<     msgs::Geometry *geomMsg = msg_number.mutable_geometry();\r\n<     geomMsg->set_type(msgs::Geometry::PLANE);\r\n---\r\n> \t\/\/visualize the estimated position of the robot every few seconds\r\n> \tfawkes::Time new_time     = clock->now();\r\n> \tdouble       time_elapsed = new_time.in_sec() - last_update_time_.in_sec();\r\n> \tif (time_elapsed > 1 \/ update_rate_) {\r\n> \t\tlast_update_time_ = new_time;\r\n> \r\n> \t\t\/\/read pose\r\n> \t\tpose_if_->read();\r\n> \t\tdouble x = pose_if_->translation(0);\r\n> \t\tdouble y = pose_if_->translation(1);\r\n> \t\t\/\/calculate ori from quaternion in interface\r\n> \t\tdouble *quat = pose_if_->rotation();\r\n> \t\tdouble  ori  = tf::get_yaw(tf::Quaternion(quat[0], quat[1], quat[2], quat[3]));\r\n> \t\tif (std::isnan(ori)) {\r\n> \t\t\tori = 0.0;\r\n> \t\t}\r\n> \r\n> \t\t\/\/create label with number\r\n> \t\tmsgs::Visual msg_number;\r\n> \t\tmsg_number.set_name((robot_name_ + "-localization-label").c_str());\r\n> \t\tmsg_number.set_parent_name(parent_name_.c_str());\r\n> \t\tmsgs::Geometry *geomMsg = msg_number.mutable_geometry();\r\n> \t\tgeomMsg->set_type(msgs::Geometry::PLANE);\r\n107,108c107,109\r\n<     msgs::Set(geomMsg->mutable_plane()->mutable_normal(), ignition::math::Vector3d(0.0, 0.0, 1.0));\r\n<     msgs::Set(geomMsg->mutable_plane()->mutable_size(), ignition::math::Vector2d(label_size_, label_size_));\r\n---\r\n> \t\tmsgs::Set(geomMsg->mutable_plane()->mutable_normal(), ignition::math::Vector3d(0.0, 0.0, 1.0));\r\n> \t\tmsgs::Set(geomMsg->mutable_plane()->mutable_size(),\r\n> \t\t          ignition::math::Vector2d(label_size_, label_size_));\r\n110,112c111,113\r\n<     msgs::Set(geomMsg->mutable_plane()->mutable_normal(), math::Vector3(0.0, 0.0, 1.0));\r\n<     msgs::Set(geomMsg->mutable_plane()->mutable_size(), math::Vector2d(label_size_, label_size_));\r\n<     msg_number.set_transparency(0.2);  \r\n---\r\n> \t\tmsgs::Set(geomMsg->mutable_plane()->mutable_normal(), math::Vector3(0.0, 0.0, 1.0));\r\n> \t\tmsgs::Set(geomMsg->mutable_plane()->mutable_size(), math::Vector2d(label_size_, label_size_));\r\n> \t\tmsg_number.set_transparency(0.2);\r\n114c115\r\n<     msg_number.set_cast_shadows(false);\r\n---\r\n> \t\tmsg_number.set_cast_shadows(false);\r\n116c117\r\n<     msgs::Set(msg_number.mutable_pose(), ignition::math::Pose3d(x, y, label_height_, 0, 0, 0));\r\n---\r\n> \t\tmsgs::Set(msg_number.mutable_pose(), ignition::math::Pose3d(x, y, label_height_, 0, 0, 0));\r\n118c119\r\n<     msgs::Set(msg_number.mutable_pose(), math::Pose(x, y, label_height_, 0, 0, 0));\r\n---\r\n> \t\tmsgs::Set(msg_number.mutable_pose(), math::Pose(x, y, label_height_, 0, 0, 0));\r\n120,123c121,124\r\n<     msgs::Material::Script* script = msg_number.mutable_material()->mutable_script();\r\n<     script->add_uri(location_scripts_.c_str());\r\n<     script->add_uri(location_textures_.c_str());\r\n<     script->set_name(label_script_name_.c_str());\r\n---\r\n> \t\tmsgs::Material::Script *script = msg_number.mutable_material()->mutable_script();\r\n> \t\tscript->add_uri(location_scripts_.c_str());\r\n> \t\tscript->add_uri(location_textures_.c_str());\r\n> \t\tscript->set_name(label_script_name_.c_str());\r\n125c126\r\n<     visual_publisher_->Publish(msg_number);\r\n---\r\n> \t\tvisual_publisher_->Publish(msg_number);\r\n127c128\r\n<     \/\/create label with direction arrow\r\n---\r\n> \t\t\/\/create label with direction arrow\r\n129,151c130,156\r\n<     msgs::Visual msg_arrow;\r\n<     msg_arrow.set_name((robot_name_ + "-localization-arrow").c_str());\r\n<     msg_arrow.set_parent_name(parent_name_.c_str());\r\n<     msgs::Geometry *geomArrowMsg = msg_arrow.mutable_geometry();\r\n<     geomArrowMsg->set_type(msgs::Geometry::PLANE);\r\n< #if GAZEBO_MAJOR_VERSION > 5\r\n<     msgs::Set(geomArrowMsg->mutable_plane()->mutable_normal(), ignition::math::Vector3d(0.0, 0.0, 1.0));\r\n<     msgs::Set(geomArrowMsg->mutable_plane()->mutable_size(), ignition::math::Vector2d(0.17, 0.17));\r\n< #else\r\n<     msgs::Set(geomArrowMsg->mutable_plane()->mutable_normal(), math::Vector3(0.0, 0.0, 1.0));\r\n<     msgs::Set(geomArrowMsg->mutable_plane()->mutable_size(), math::Vector2d(0.17, 0.17));\r\n<     msg_arrow.set_transparency(0.4);  \r\n< #endif\r\n<     msg_arrow.set_cast_shadows(false);\r\n< #if GAZEBO_MAJOR_VERSION > 5\r\n<     msgs::Set(msg_arrow.mutable_pose(), ignition::math::Pose3d(x, y, label_height_ + 0.01, 0, 0, ori - \/*turn image right*\/ M_PI \/ 2));\r\n< #else\r\n<     msgs::Set(msg_arrow.mutable_pose(), math::Pose(x, y, label_height_ + 0.01, 0, 0, ori - \/*turn image right*\/ M_PI \/ 2));\r\n< #endif\r\n<     msgs::Material::Script* arrow_script = msg_arrow.mutable_material()->mutable_script();\r\n<     arrow_script->add_uri(location_scripts_.c_str());\r\n<     arrow_script->add_uri(location_textures_.c_str());\r\n<     arrow_script->set_name(arrow_script_name_.c_str());\r\n---\r\n> \t\tmsgs::Visual msg_arrow;\r\n> \t\tmsg_arrow.set_name((robot_name_ + "-localization-arrow").c_str());\r\n> \t\tmsg_arrow.set_parent_name(parent_name_.c_str());\r\n> \t\tmsgs::Geometry *geomArrowMsg = msg_arrow.mutable_geometry();\r\n> \t\tgeomArrowMsg->set_type(msgs::Geometry::PLANE);\r\n> #\tif GAZEBO_MAJOR_VERSION > 5\r\n> \t\tmsgs::Set(geomArrowMsg->mutable_plane()->mutable_normal(),\r\n> \t\t          ignition::math::Vector3d(0.0, 0.0, 1.0));\r\n> \t\tmsgs::Set(geomArrowMsg->mutable_plane()->mutable_size(), ignition::math::Vector2d(0.17, 0.17));\r\n> #\telse\r\n> \t\tmsgs::Set(geomArrowMsg->mutable_plane()->mutable_normal(), math::Vector3(0.0, 0.0, 1.0));\r\n> \t\tmsgs::Set(geomArrowMsg->mutable_plane()->mutable_size(), math::Vector2d(0.17, 0.17));\r\n> \t\tmsg_arrow.set_transparency(0.4);\r\n> #\tendif\r\n> \t\tmsg_arrow.set_cast_shadows(false);\r\n> #\tif GAZEBO_MAJOR_VERSION > 5\r\n> \t\tmsgs::Set(msg_arrow.mutable_pose(),\r\n> \t\t          ignition::math::Pose3d(\r\n> \t\t            x, y, label_height_ + 0.01, 0, 0, ori - \/*turn image right*\/ M_PI \/ 2));\r\n> #\telse\r\n> \t\tmsgs::Set(msg_arrow.mutable_pose(),\r\n> \t\t          math::Pose(x, y, label_height_ + 0.01, 0, 0, ori - \/*turn image right*\/ M_PI \/ 2));\r\n> #\tendif\r\n> \t\tmsgs::Material::Script *arrow_script = msg_arrow.mutable_material()->mutable_script();\r\n> \t\tarrow_script->add_uri(location_scripts_.c_str());\r\n> \t\tarrow_script->add_uri(location_textures_.c_str());\r\n> \t\tarrow_script->set_name(arrow_script_name_.c_str());\r\n153c158\r\n<     visual_publisher_->Publish(msg_arrow);\r\n---\r\n> \t\tvisual_publisher_->Publish(msg_arrow);\r\n155c160\r\n<   }\r\n---\r\n> \t}\r\ndiff -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\r\n21,22c21,22\r\n< #ifndef __PLUGINS_GAZSIM_VIS_LOCALIZATION_THREAD_H_\r\n< #define __PLUGINS_GAZSIM_VIS_LOCALIZATION_THREAD_H_\r\n---\r\n> #ifndef _PLUGINS_GAZSIM_VIS_LOCALIZATION_THREAD_H_\r\n> #define _PLUGINS_GAZSIM_VIS_LOCALIZATION_THREAD_H_\r\n24c24,25\r\n< #include <core\/threading\/thread.h>\r\n---\r\n> #include <aspect\/blackboard.h>\r\n> #include <aspect\/blocked_timing.h>\r\n28,29c29\r\n< #include <aspect\/blackboard.h>\r\n< #include <aspect\/blocked_timing.h>\r\n---\r\n> #include <core\/threading\/thread.h>\r\n32a33\r\n> \r\n36d36\r\n< #include <gazebo\/transport\/TransportTypes.hh>\r\n37a38\r\n> #include <gazebo\/transport\/TransportTypes.hh>\r\n40d40\r\n< \r\n42c42\r\n<   class Position3DInterface;\r\n---\r\n> class Position3DInterface;\r\n45,52c45,51\r\n< class VisLocalizationThread\r\n< : public fawkes::Thread,\r\n<   public fawkes::ClockAspect,\r\n<   public fawkes::LoggingAspect,\r\n<   public fawkes::ConfigurableAspect,\r\n<   public fawkes::BlackBoardAspect,\r\n<   public fawkes::BlockedTimingAspect,\r\n<   public fawkes::GazeboAspect\r\n---\r\n> class VisLocalizationThread : public fawkes::Thread,\r\n>                               public fawkes::ClockAspect,\r\n>                               public fawkes::LoggingAspect,\r\n>                               public fawkes::ConfigurableAspect,\r\n>                               public fawkes::BlackBoardAspect,\r\n>                               public fawkes::BlockedTimingAspect,\r\n>                               public fawkes::GazeboAspect\r\n54,55c53,54\r\n<  public:\r\n<   VisLocalizationThread();\r\n---\r\n> public:\r\n> \tVisLocalizationThread();\r\n57,74c56,74\r\n<   virtual void init();\r\n<   virtual void loop();\r\n<   virtual void finalize();\r\n< \r\n<  private:\r\n<   \/\/read pose interface\r\n<   fawkes::Position3DInterface *pose_if_;\r\n< \r\n<   \/\/Publisher for visual msgs\r\n<   gazebo::transport::PublisherPtr visual_publisher_;\r\n< \r\n<   double update_rate_;\r\n<   fawkes::Time last_update_time_;\r\n< \r\n<   \/\/config values\r\n<   std::string robot_name_, label_script_name_, location_scripts_, location_textures_, parent_name_, arrow_script_name_;\r\n<   float label_size_;\r\n<   float label_height_;\r\n---\r\n> \tvirtual void init();\r\n> \tvirtual void loop();\r\n> \tvirtual void finalize();\r\n> \r\n> private:\r\n> \t\/\/read pose interface\r\n> \tfawkes::Position3DInterface *pose_if_;\r\n> \r\n> \t\/\/Publisher for visual msgs\r\n> \tgazebo::transport::PublisherPtr visual_publisher_;\r\n> \r\n> \tdouble       update_rate_;\r\n> \tfawkes::Time last_update_time_;\r\n> \r\n> \t\/\/config values\r\n> \tstd::string robot_name_, label_script_name_, location_scripts_, location_textures_, parent_name_,\r\n> \t  arrow_script_name_;\r\n> \tfloat label_size_;\r\n> \tfloat label_height_;\r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/gazsim-vis-localization\/Makefile fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/gazsim-vis-localization\/Makefile\r\n25c25\r\n< LIBS_gazsim_vis_localization = fawkescore fawkesutils fawkesaspects fvutils \\\r\n---\r\n> LIBS_gazsim_vis_localization = m fawkescore fawkesutils fawkesaspects fvutils \\\r\n31a32\r\n> PLUGINS_all = $(PLUGINDIR)\/gazsim-vis-localization.so\r\n37c38,39\r\n<   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) -lm $(call boost-libs-ldflags,system) -lboost_system\r\n---\r\n>   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) \\\r\n>              $(call boost-libs-ldflags,system)\r\n39c41\r\n<   PLUGINS_all = $(PLUGINDIR)\/gazsim-vis-localization.so\r\n---\r\n>   PLUGINS_build = $(PLUGINS_all)\r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/gazsim-webcam\/gazsim_webcam.cpp fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/gazsim-webcam\/gazsim_webcam.cpp\r\n22a23\r\n> #include <fvutils\/color\/conversions.h>\r\n24,25d24\r\n< #include <stdio.h>\r\n< #include <math.h>\r\n28d26\r\n< #include <gazebo\/transport\/Node.hh>\r\n29a28\r\n> #include <gazebo\/transport\/Node.hh>\r\n31,32c30,31\r\n< \r\n< #include <fvutils\/color\/conversions.h>\r\n---\r\n> #include <math.h>\r\n> #include <stdio.h>\r\n41d39\r\n< \r\n48,50c46,48\r\n< GazsimWebcam::GazsimWebcam(std::string shm_id,\r\n< \t\t\t     gazebo::transport::NodePtr gazebo_world_node,\r\n< \t\t\t     Configuration* config)\r\n---\r\n> GazsimWebcam::GazsimWebcam(std::string                shm_id,\r\n>                            gazebo::transport::NodePtr gazebo_world_node,\r\n>                            Configuration *            config)\r\n52,84c50,78\r\n<   shm_buffer_ = NULL;\r\n<   \/\/read config values\r\n<   std::string robot_name = config->get_string("\/gazsim\/robot-name");\r\n<   shm_id_ = robot_name + "\/" + shm_id;\r\n<   topic_name_ = ("~\/" \r\n< \t\t + robot_name\r\n<                  + config->get_string((std::string("\/gazsim\/webcam\/topic-suffixes\/")\r\n< \t\t                        + shm_id).c_str()));\r\n<   width_ = config->get_float((std::string("\/gazsim\/webcam\/widths\/") + shm_id).c_str());\r\n<   height_ = config->get_float((std::string("\/gazsim\/webcam\/heights\/") + shm_id).c_str());\r\n<   frame_ = config->get_string((std::string("\/gazsim\/webcam\/frames\/") + shm_id).c_str());\r\n< \r\n<   format_from_ = firevision::RGB;\r\n<   format_to_ = firevision::YUV422_PLANAR;\r\n< \r\n<   \/\/subscribing to gazebo publisher\r\n<   \/\/the messages are published by the sensor itself and not by a robot plugin\r\n<   \/\/therefore we have to use the world node\r\n<   webcam_sub_ = gazebo_world_node->Subscribe(topic_name_, &GazsimWebcam::on_webcam_data_msg, this);\r\n< \r\n<   \/\/initialize shared memory image buffer\r\n<   shm_buffer_ = new firevision::SharedMemoryImageBuffer( shm_id_.c_str(),\r\n< \t\t\t\t\t\t\t format_to_,\r\n< \t\t\t\t\t\t\t width_,\r\n< \t\t\t\t\t\t\t height_\r\n< \t\t\t\t\t\t\t );\r\n<   if (!shm_buffer_->is_valid()) {\r\n<     throw fawkes::Exception("Shared memory segment not valid");\r\n<   }\r\n<   shm_buffer_->set_frame_id(frame_.c_str());\r\n<   buffer_ = shm_buffer_->buffer();\r\n<   \/\/enable locking\r\n<   shm_buffer_->add_semaphore();\r\n---\r\n> \tshm_buffer_ = NULL;\r\n> \t\/\/read config values\r\n> \tstd::string robot_name = config->get_string("\/gazsim\/robot-name");\r\n> \tshm_id_                = robot_name + "\/" + shm_id;\r\n> \ttopic_name_ =\r\n> \t  ("~\/" + robot_name\r\n> \t   + config->get_string((std::string("\/gazsim\/webcam\/topic-suffixes\/") + shm_id).c_str()));\r\n> \twidth_  = config->get_float((std::string("\/gazsim\/webcam\/widths\/") + shm_id).c_str());\r\n> \theight_ = config->get_float((std::string("\/gazsim\/webcam\/heights\/") + shm_id).c_str());\r\n> \tframe_  = config->get_string((std::string("\/gazsim\/webcam\/frames\/") + shm_id).c_str());\r\n> \r\n> \tformat_from_ = firevision::RGB;\r\n> \tformat_to_   = firevision::YUV422_PLANAR;\r\n> \r\n> \t\/\/subscribing to gazebo publisher\r\n> \t\/\/the messages are published by the sensor itself and not by a robot plugin\r\n> \t\/\/therefore we have to use the world node\r\n> \twebcam_sub_ = gazebo_world_node->Subscribe(topic_name_, &GazsimWebcam::on_webcam_data_msg, this);\r\n> \r\n> \t\/\/initialize shared memory image buffer\r\n> \tshm_buffer_ =\r\n> \t  new firevision::SharedMemoryImageBuffer(shm_id_.c_str(), format_to_, width_, height_);\r\n> \tif (!shm_buffer_->is_valid()) {\r\n> \t\tthrow fawkes::Exception("Shared memory segment not valid");\r\n> \t}\r\n> \tshm_buffer_->set_frame_id(frame_.c_str());\r\n> \tbuffer_ = shm_buffer_->buffer();\r\n> \t\/\/enable locking\r\n> \tshm_buffer_->add_semaphore();\r\n89c83\r\n<   delete this->shm_buffer_;\r\n---\r\n> \tdelete this->shm_buffer_;\r\n92,100c86,98\r\n< void GazsimWebcam::on_webcam_data_msg(ConstImageStampedPtr &msg)\r\n< {  \r\n<   \/\/convert image data and write it in the shared memory buffer\r\n<   \/\/lock the shm so noone can read a half written image\r\n<   shm_buffer_->lock_for_write();\r\n<   convert(format_from_,  format_to_,\r\n<  \t  (const unsigned char*) msg->image().data().data(),   buffer_,\r\n< \t  width_, height_);\r\n<   shm_buffer_->unlock();\r\n---\r\n> void\r\n> GazsimWebcam::on_webcam_data_msg(ConstImageStampedPtr &msg)\r\n> {\r\n> \t\/\/convert image data and write it in the shared memory buffer\r\n> \t\/\/lock the shm so noone can read a half written image\r\n> \tshm_buffer_->lock_for_write();\r\n> \tconvert(format_from_,\r\n> \t        format_to_,\r\n> \t        (const unsigned char *)msg->image().data().data(),\r\n> \t        buffer_,\r\n> \t        width_,\r\n> \t        height_);\r\n> \tshm_buffer_->unlock();\r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/gazsim-webcam\/gazsim_webcam.h fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/gazsim-webcam\/gazsim_webcam.h\r\n21,22c21,22\r\n< #ifndef __PLUGINS_GAZSIM_WEBCAM_H_\r\n< #define __PLUGINS_GAZSIM_WEBCAM_H_\r\n---\r\n> #ifndef _PLUGINS_GAZSIM_WEBCAM_H_\r\n> #define _PLUGINS_GAZSIM_WEBCAM_H_\r\n24c24,25\r\n< #include <core\/threading\/thread.h>\r\n---\r\n> #include <aspect\/blackboard.h>\r\n> #include <aspect\/blocked_timing.h>\r\n28,31d28\r\n< #include <aspect\/blackboard.h>\r\n< #include <aspect\/blocked_timing.h>\r\n< #include <plugins\/gazebo\/aspect\/gazebo.h>\r\n< #include <string.h>\r\n33c30\r\n< \r\n---\r\n> #include <core\/threading\/thread.h>\r\n34a32,33\r\n> #include <plugins\/gazebo\/aspect\/gazebo.h>\r\n> \r\n35a35\r\n> #include <string.h>\r\n38d37\r\n< #include <gazebo\/transport\/TransportTypes.hh>\r\n39a39\r\n> #include <gazebo\/transport\/TransportTypes.hh>\r\n42d41\r\n< \r\n45,71c44,70\r\n<  public:\r\n<   GazsimWebcam(std::string shm_id,\r\n< \t       gazebo::transport::NodePtr gazebo_world_node,\r\n< \t       fawkes::Configuration* config);\r\n<   ~GazsimWebcam();\r\n< \r\n<  private:  \r\n<   \/\/Subscriber to receive webcam data from gazebo\r\n<   gazebo::transport::SubscriberPtr webcam_sub_;\r\n< \r\n<   \/\/handler function for incoming webcam data messages\r\n<   void on_webcam_data_msg(ConstImageStampedPtr &msg);\r\n< \r\n<   \/\/shared memory buffer\r\n<   firevision::SharedMemoryImageBuffer *shm_buffer_;\r\n<   \/\/reference to the buffer of shm_buffer_\r\n<   unsigned char *buffer_;\r\n< \r\n<   \/\/config values\r\n<   \/\/topic name of the gazebo message publisher\r\n<   std::string topic_name_;\r\n<   double width_, height_;\r\n<   \/\/id of the shared memory object\r\n<   std::string shm_id_;\r\n<   std::string frame_;\r\n<   firevision::colorspace_t format_from_;\r\n<   firevision::colorspace_t format_to_;\r\n---\r\n> public:\r\n> \tGazsimWebcam(std::string                shm_id,\r\n> \t             gazebo::transport::NodePtr gazebo_world_node,\r\n> \t             fawkes::Configuration *    config);\r\n> \t~GazsimWebcam();\r\n> \r\n> private:\r\n> \t\/\/Subscriber to receive webcam data from gazebo\r\n> \tgazebo::transport::SubscriberPtr webcam_sub_;\r\n> \r\n> \t\/\/handler function for incoming webcam data messages\r\n> \tvoid on_webcam_data_msg(ConstImageStampedPtr &msg);\r\n> \r\n> \t\/\/shared memory buffer\r\n> \tfirevision::SharedMemoryImageBuffer *shm_buffer_;\r\n> \t\/\/reference to the buffer of shm_buffer_\r\n> \tunsigned char *buffer_;\r\n> \r\n> \t\/\/config values\r\n> \t\/\/topic name of the gazebo message publisher\r\n> \tstd::string topic_name_;\r\n> \tdouble      width_, height_;\r\n> \t\/\/id of the shared memory object\r\n> \tstd::string              shm_id_;\r\n> \tstd::string              frame_;\r\n> \tfirevision::colorspace_t format_from_;\r\n> \tfirevision::colorspace_t format_to_;\r\ndiff -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\r\n22,23d21\r\n< #include <core\/plugin.h>\r\n< \r\n25a24,25\r\n> #include <core\/plugin.h>\r\n> \r\n35,37c35,36\r\n<  public:\r\n< \r\n<   \/** Constructor.\r\n---\r\n> public:\r\n> \t\/** Constructor.\r\n40,44c39,42\r\n<   GazsimWebcamPlugin(Configuration *config)\r\n<     : Plugin(config)\r\n<   {\r\n<     thread_list.push_back(new WebcamSimThread());\r\n<   }\r\n---\r\n> \texplicit GazsimWebcamPlugin(Configuration *config) : Plugin(config)\r\n> \t{\r\n> \t\tthread_list.push_back(new WebcamSimThread());\r\n> \t}\r\ndiff -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\r\n23a24,25\r\n> #include <aspect\/logging.h>\r\n> #include <fvutils\/color\/conversions.h>\r\n25,26d26\r\n< #include <stdio.h>\r\n< #include <math.h>\r\n29d28\r\n< #include <gazebo\/transport\/Node.hh>\r\n30a30\r\n> #include <gazebo\/transport\/Node.hh>\r\n32,34c32,33\r\n< #include <aspect\/logging.h>\r\n< \r\n< #include <fvutils\/color\/conversions.h>\r\n---\r\n> #include <math.h>\r\n> #include <stdio.h>\r\n46,47c45,46\r\n<   : Thread("WebcamSimThread", Thread::OPMODE_WAITFORWAKEUP),\r\n<     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)\r\n---\r\n> : Thread("WebcamSimThread", Thread::OPMODE_WAITFORWAKEUP),\r\n>   BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)\r\n51c50,51\r\n< void WebcamSimThread::init()\r\n---\r\n> void\r\n> WebcamSimThread::init()\r\n53,54c53,54\r\n<   logger->log_debug(name(), "Initializing Simulation of the Webcams");\r\n<   shm_ids_ = config->get_strings("\/gazsim\/webcam\/shm-image-ids");\r\n---\r\n> \tlogger->log_debug(name(), "Initializing Simulation of the Webcams");\r\n> \tshm_ids_ = config->get_strings("\/gazsim\/webcam\/shm-image-ids");\r\n56,59c56,58\r\n<   for (std::vector<std::string>::iterator it = shm_ids_.begin(); it != shm_ids_.end(); ++it)\r\n<   {\r\n<     webcams_.push_back(new GazsimWebcam(*it, gazebo_world_node, config));\r\n<   }\r\n---\r\n> \tfor (std::vector<std::string>::iterator it = shm_ids_.begin(); it != shm_ids_.end(); ++it) {\r\n> \t\twebcams_.push_back(new GazsimWebcam(*it, gazebo_world_node, config));\r\n> \t}\r\n62c61,62\r\n< void WebcamSimThread::finalize()\r\n---\r\n> void\r\n> WebcamSimThread::finalize()\r\n64,67c64,66\r\n<   for (std::vector<GazsimWebcam*>::iterator it = webcams_.begin(); it != webcams_.end(); ++it)\r\n<   {\r\n<     delete *it;\r\n<   }  \r\n---\r\n> \tfor (std::vector<GazsimWebcam *>::iterator it = webcams_.begin(); it != webcams_.end(); ++it) {\r\n> \t\tdelete *it;\r\n> \t}\r\n70c69,70\r\n< void WebcamSimThread::loop()\r\n---\r\n> void\r\n> WebcamSimThread::loop()\r\n72c72\r\n<   \/\/The interesting stuff happens in the callback of the webcams\r\n---\r\n> \t\/\/The interesting stuff happens in the callback of the webcams\r\ndiff -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\r\n22,23c22,23\r\n< #ifndef __PLUGINS_GAZSIM_WEBCAM_THREAD_H_\r\n< #define __PLUGINS_GAZSIM_WEBCAM_THREAD_H_\r\n---\r\n> #ifndef _PLUGINS_GAZSIM_WEBCAM_THREAD_H_\r\n> #define _PLUGINS_GAZSIM_WEBCAM_THREAD_H_\r\n25c25,26\r\n< #include <core\/threading\/thread.h>\r\n---\r\n> #include <aspect\/blackboard.h>\r\n> #include <aspect\/blocked_timing.h>\r\n29,30c30,31\r\n< #include <aspect\/blackboard.h>\r\n< #include <aspect\/blocked_timing.h>\r\n---\r\n> #include <core\/threading\/thread.h>\r\n> #include <fvutils\/ipc\/shm_image.h>\r\n32d32\r\n< #include <string.h>\r\n34d33\r\n< #include <fvutils\/ipc\/shm_image.h>\r\n35a35\r\n> #include <string.h>\r\n38c38,39\r\n< #include <gazebo\/transport\/TransportTypes.hh>\r\n---\r\n> #include "gazsim_webcam.h"\r\n> \r\n39a41\r\n> #include <gazebo\/transport\/TransportTypes.hh>\r\n42,51c44,50\r\n< #include "gazsim_webcam.h"\r\n< \r\n< class WebcamSimThread\r\n< : public fawkes::Thread,\r\n<   public fawkes::ClockAspect,\r\n<   public fawkes::LoggingAspect,\r\n<   public fawkes::ConfigurableAspect,\r\n<   public fawkes::BlackBoardAspect,\r\n<   public fawkes::BlockedTimingAspect,\r\n<   public fawkes::GazeboAspect\r\n---\r\n> class WebcamSimThread : public fawkes::Thread,\r\n>                         public fawkes::ClockAspect,\r\n>                         public fawkes::LoggingAspect,\r\n>                         public fawkes::ConfigurableAspect,\r\n>                         public fawkes::BlackBoardAspect,\r\n>                         public fawkes::BlockedTimingAspect,\r\n>                         public fawkes::GazeboAspect\r\n53,54c52,60\r\n<  public:\r\n<   WebcamSimThread();\r\n---\r\n> public:\r\n> \tWebcamSimThread();\r\n> \r\n> \tvirtual void init();\r\n> \tvirtual void loop();\r\n> \tvirtual void finalize();\r\n> \r\n> private:\r\n> \tstd::vector<GazsimWebcam *> webcams_;\r\n56,64c62,63\r\n<   virtual void init();\r\n<   virtual void loop();\r\n<   virtual void finalize();\r\n< \r\n<  private:\r\n<   std::vector<GazsimWebcam*> webcams_;\r\n<   \r\n<   \/\/config values\r\n<   std::vector<std::string> shm_ids_;\r\n---\r\n> \t\/\/config values\r\n> \tstd::vector<std::string> shm_ids_;\r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/gazsim-webcam\/Makefile fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/gazsim-webcam\/Makefile\r\n25c25\r\n< LIBS_gazsim_webcam = fawkescore fawkesutils fawkesaspects fvutils \\\r\n---\r\n> LIBS_gazsim_webcam = m fawkescore fawkesutils fawkesaspects fvutils \\\r\n30a31\r\n> PLUGINS_all = $(PLUGINDIR)\/gazsim-webcam.so\r\n36c37,38\r\n<   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) -lm $(call boost-libs-ldflags,system) -lboost_system\r\n---\r\n>   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) \\\r\n>              $(call boost-libs-ldflags,system)\r\n38c40\r\n<   PLUGINS_all = $(PLUGINDIR)\/gazsim-webcam.so\r\n---\r\n>   PLUGINS_build = $(PLUGINS_all)\r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/Makefile fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/Makefile\r\n26,27c26,27\r\n< \t  gazsim-vis-localization gazsim-webcam gazsim-timesource \\\r\n< \t  gazsim-depthcam\r\n---\r\n>           gazsim-vis-localization gazsim-webcam gazsim-timesource \\\r\n>           gazsim-depthcam\r\n29c29\r\n< LIBS_gazebo = fawkescore fawkesaspects fawkesgazeboaspect\r\n---\r\n> LIBS_gazebo = m fawkescore fawkesaspects fawkesgazeboaspect\r\n32a33\r\n> PLUGINS_all = $(PLUGINDIR)\/gazebo.$(SOEXT)\r\n36c37,38\r\n<   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) -lm $(call boost-libs-ldflags,system)\r\n---\r\n>   LDFLAGS += $(LDFLAGS_GAZEBO) $(LDFLAGS_PROTOBUF) \\\r\n>              $(call boost-libs-ldflags,system)\r\n38c40\r\n<   PLUGINS_all = $(PLUGINDIR)\/gazebo.$(SOEXT)\r\n---\r\n>   PLUGINS_build = $(PLUGINS_all)\r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/node_thread.cpp fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/node_thread.cpp\r\n26,28d25\r\n< #include <gazebo\/transport\/TransportIface.hh>\r\n< #include <gazebo\/transport\/TransportTypes.hh>\r\n< #include <gazebo\/transport\/Node.hh>\r\n31d27\r\n< #include <gazebo\/msgs\/msgs.hh>\r\n32a29,32\r\n> #include <gazebo\/msgs\/msgs.hh>\r\n> #include <gazebo\/transport\/Node.hh>\r\n> #include <gazebo\/transport\/TransportIface.hh>\r\n> #include <gazebo\/transport\/TransportTypes.hh>\r\n46,48c46,48\r\n<   : Thread("GazeboNodeThread", Thread::OPMODE_WAITFORWAKEUP),\r\n<     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_POST_LOOP),\r\n<     AspectProviderAspect(&__gazebo_aspect_inifin)\r\n---\r\n> : Thread("GazeboNodeThread", Thread::OPMODE_WAITFORWAKEUP),\r\n>   BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_POST_LOOP),\r\n>   AspectProviderAspect(&gazebo_aspect_inifin_)\r\n52d51\r\n< \r\n58d56\r\n< \r\n62,87c60,84\r\n<   \/\/read config values\r\n<   robot_channel = config->get_string("\/gazsim\/world-name")\r\n<     + "\/" + config->get_string("\/gazsim\/robot-name");\r\n<   \r\n<   world_name = config->get_string("\/gazsim\/world-name");\r\n< \r\n<   if(gazebo::transport::is_stopped()) {\r\n<     gazebo::transport::init();\r\n<     gazebo::transport::run();\r\n<   }\r\n<   else {\r\n<     logger->log_warn(name(), "Gazebo already running ");\r\n<   }\r\n< \r\n<   \/\/Initialize Communication nodes:\r\n<   \/\/the common one for the robot\r\n<   gazebo::transport::NodePtr node(new gazebo::transport::Node());\r\n<   __gazebonode = node;\r\n<   \/\/initialize node (this node only communicates with nodes that were initialized with the same string)\r\n<   __gazebonode->Init(robot_channel.c_str());\r\n<   __gazebo_aspect_inifin.set_gazebonode(__gazebonode);\r\n<   \r\n<   \/\/and the node for world change messages\r\n<   __gazebo_world_node = gazebo::transport::NodePtr(new gazebo::transport::Node());\r\n<   __gazebo_world_node->Init(world_name.c_str());\r\n<   __gazebo_aspect_inifin.set_gazebo_world_node(__gazebo_world_node);\r\n---\r\n> \t\/\/read config values\r\n> \trobot_channel =\r\n> \t  config->get_string("\/gazsim\/world-name") + "\/" + config->get_string("\/gazsim\/robot-name");\r\n> \r\n> \tworld_name = config->get_string("\/gazsim\/world-name");\r\n> \r\n> \tif (gazebo::transport::is_stopped()) {\r\n> \t\tgazebo::transport::init();\r\n> \t\tgazebo::transport::run();\r\n> \t} else {\r\n> \t\tlogger->log_warn(name(), "Gazebo already running ");\r\n> \t}\r\n> \r\n> \t\/\/Initialize Communication nodes:\r\n> \t\/\/the common one for the robot\r\n> \tgazebo::transport::NodePtr node(new gazebo::transport::Node());\r\n> \tgazebonode_ = node;\r\n> \t\/\/initialize node (this node only communicates with nodes that were initialized with the same string)\r\n> \tgazebonode_->Init(robot_channel.c_str());\r\n> \tgazebo_aspect_inifin_.set_gazebonode(gazebonode_);\r\n> \r\n> \t\/\/and the node for world change messages\r\n> \tgazebo_world_node_ = gazebo::transport::NodePtr(new gazebo::transport::Node());\r\n> \tgazebo_world_node_->Init(world_name.c_str());\r\n> \tgazebo_aspect_inifin_.set_gazebo_world_node(gazebo_world_node_);\r\n90d86\r\n< \r\n94,99c90,95\r\n<   __gazebonode->Fini();\r\n<   __gazebonode.reset();\r\n<   __gazebo_aspect_inifin.set_gazebonode(__gazebonode);\r\n<   __gazebo_world_node->Fini();\r\n<   __gazebo_world_node.reset();\r\n<   __gazebo_aspect_inifin.set_gazebonode(__gazebo_world_node);\r\n---\r\n> \tgazebonode_->Fini();\r\n> \tgazebonode_.reset();\r\n> \tgazebo_aspect_inifin_.set_gazebonode(gazebonode_);\r\n> \tgazebo_world_node_->Fini();\r\n> \tgazebo_world_node_.reset();\r\n> \tgazebo_aspect_inifin_.set_gazebonode(gazebo_world_node_);\r\n101d96\r\n< \r\ndiff -r fawkes-robotino\/fawkes\/src\/plugins\/gazebo\/node_thread.h fawkes-robotino-2016\/fawkes\/src\/plugins\/gazebo\/node_thread.h\r\n23,24c23,24\r\n< #ifndef __PLUGINS_GAZEBO_NODE_THREAD_H_\r\n< #define __PLUGINS_GAZEBO_NODE_THREAD_H_\r\n---\r\n> #ifndef _PLUGINS_GAZEBO_NODE_THREAD_H_\r\n> #define _PLUGINS_GAZEBO_NODE_THREAD_H_\r\n26,30d25\r\n< #include <core\/threading\/thread.h>\r\n< #include <aspect\/logging.h>\r\n< #include <aspect\/configurable.h>\r\n< #include <aspect\/clock.h>\r\n< #include <aspect\/blocked_timing.h>\r\n31a27,31\r\n> #include <aspect\/blocked_timing.h>\r\n> #include <aspect\/clock.h>\r\n> #include <aspect\/configurable.h>\r\n> #include <aspect\/logging.h>\r\n> #include <core\/threading\/thread.h>\r\n32a33\r\n> #include <sys\/types.h>\r\n34d34\r\n< #include <string.h>\r\n36c36\r\n< #include <sys\/types.h>\r\n---\r\n> #include <string.h>\r\n39,41c39,40\r\n<   namespace transport {\r\n<     class Node;\r\n<   }\r\n---\r\n> namespace transport {\r\n> class Node;\r\n42a42\r\n> } \/\/ namespace gazebo\r\n44,50c44,49\r\n< class GazeboNodeThread\r\n< : public fawkes::Thread,\r\n<   public fawkes::BlockedTimingAspect,\r\n<   public fawkes::LoggingAspect,\r\n<   public fawkes::ConfigurableAspect,\r\n<   public fawkes::ClockAspect,\r\n<   public fawkes::AspectProviderAspect\r\n---\r\n> class GazeboNodeThread : public fawkes::Thread,\r\n>                          public fawkes::BlockedTimingAspect,\r\n>                          public fawkes::LoggingAspect,\r\n>                          public fawkes::ConfigurableAspect,\r\n>                          public fawkes::ClockAspect,\r\n>                          public fawkes::AspectProviderAspect\r\n52,69c51,74\r\n<  public:\r\n<   GazeboNodeThread();\r\n<   virtual ~GazeboNodeThread();\r\n< \r\n<   virtual void init();\r\n<   virtual void loop();\r\n<   virtual void finalize();\r\n< \r\n<  \/** Stub to see name in backtrace for easier debugging. @see Thread::run() *\/\r\n<  protected: virtual void run() { Thread::run(); }\r\n< \r\n<  private:\r\n<   \/\/Node for communication to gazebo-robot-plugins\r\n<   gazebo::transport::NodePtr  __gazebonode;\r\n<   \/\/Node to control the gazebo world (e.g. spawn visual objects)\r\n<   gazebo::transport::NodePtr  __gazebo_world_node;\r\n<   \/\/Publisher to send Messages:\r\n<   gazebo::transport::PublisherPtr __visual_publisher, __model_publisher, __request_publisher, __light_publisher;\r\n---\r\n> public:\r\n> \tGazeboNodeThread();\r\n> \tvirtual ~GazeboNodeThread();\r\n> \r\n> \tvirtual void init();\r\n> \tvirtual void loop();\r\n> \tvirtual void finalize();\r\n> \r\n> \t\/** Stub to see name in backtrace for easier debugging. @see Thread::run() *\/\r\n> protected:\r\n> \tvirtual void\r\n> \trun()\r\n> \t{\r\n> \t\tThread::run();\r\n> \t}\r\n> \r\n> private:\r\n> \t\/\/Node for communication to gazebo-robot-plugins\r\n> \tgazebo::transport::NodePtr gazebonode_;\r\n> \t\/\/Node to control the gazebo world (e.g. spawn visual objects)\r\n> \tgazebo::transport::NodePtr gazebo_world_node_;\r\n> \t\/\/Publisher to send Messages:\r\n> \tgazebo::transport::PublisherPtr visual_publisher_, model_publisher_, request_publisher_,\r\n> \t  light_publisher_;\r\n71c76\r\n<   fawkes::GazeboAspectIniFin  __gazebo_aspect_inifin;\r\n---\r\n> \tfawkes::GazeboAspectIniFin gazebo_aspect_inifin_;\r\n73,74c78,79\r\n<   \/\/channel of a specified robot for the gazebo node communication\r\n<   std::string robot_channel, world_name;\r\n---\r\n> \t\/\/channel of a specified robot for the gazebo node communication\r\n> \tstd::string robot_channel, world_name;\r\ndiff -r fawkes-robotino\/src\/plugins\/gazebo\/Makefile fawkes-robotino-2016\/src\/plugins\/gazebo\/Makefile\r\n21c21\r\n< \t  gazsim-machine-signal gazsim-gripper \\\r\n---\r\n> \t  gazsim-gripper \\\r\n23a24\r\n> # gazsim-machine-signal\r\n<\/pre>\n","protected":false},"excerpt":{"rendered":"
Ubuntu 18.04\u3078\u306efawkes-robotino \u306e\u30a4\u30f3\u30b9\u30c8\u30fc\u30eb\uff08\u8a66\u884c …<\/p>\n
  fawkes-robotino-2016\u306egazebo9\u4ee5\u964d\u3078\u306e\u5bfe\u5fdc<\/span> \u7d9a\u304d\u3092\u8aad\u3080 »<\/a><\/p>\n","protected":false},"author":1,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"_crdt_document":"","footnotes":""},"categories":[10,3,2],"tags":[],"class_list":["post-2648","post","type-post","status-publish","format-standard","hentry","category-fawkes","category-logistics-league","category-robocup"],"_links":{"self":[{"href":"https:\/\/www.kdel.org\/wp\/index.php?rest_route=\/wp\/v2\/posts\/2648","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/www.kdel.org\/wp\/index.php?rest_route=\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/www.kdel.org\/wp\/index.php?rest_route=\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/www.kdel.org\/wp\/index.php?rest_route=\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"https:\/\/www.kdel.org\/wp\/index.php?rest_route=%2Fwp%2Fv2%2Fcomments&post=2648"}],"version-history":[{"count":3,"href":"https:\/\/www.kdel.org\/wp\/index.php?rest_route=\/wp\/v2\/posts\/2648\/revisions"}],"predecessor-version":[{"id":2953,"href":"https:\/\/www.kdel.org\/wp\/index.php?rest_route=\/wp\/v2\/posts\/2648\/revisions\/2953"}],"wp:attachment":[{"href":"https:\/\/www.kdel.org\/wp\/index.php?rest_route=%2Fwp%2Fv2%2Fmedia&parent=2648"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/www.kdel.org\/wp\/index.php?rest_route=%2Fwp%2Fv2%2Fcategories&post=2648"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/www.kdel.org\/wp\/index.php?rest_route=%2Fwp%2Fv2%2Ftags&post=2648"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}