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