From 56c120ef8d4844576ead633fb2eedf60ca4c4a1a Mon Sep 17 00:00:00 2001
From: Manos Angelidis <angelidis@fortiss.org>
Date: Wed, 6 Nov 2019 15:14:22 +0000
Subject: [PATCH] Merged in ubuntu18 (pull request #31)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
[NRRPLT-6970] Update gzbridge to OSRF repository
ref. ec3a56cb7f10597fc639d2e7980968bd49334abc
Approved-by: Florian Grötzner <groetzner@fortiss.org>
Approved-by: Ugo Albanese <ugo.albanese@santannapisa.it>
---
deploy-gzbridge-nrp.sh | 9 -
gzbridge/CMakeLists.txt | 12 -
gzbridge/ConfigLoader.hh | 4 +-
gzbridge/GZNode.cc | 339 ++++++++++-----------
gzbridge/GZNode.hh | 71 ++---
gzbridge/GazeboInterface.cc | 534 +++++++++++++++++++++------------
gzbridge/GazeboInterface.hh | 116 ++++---
gzbridge/OgreMaterialParser.cc | 7 +-
gzbridge/OgreMaterialParser.hh | 6 +-
gzbridge/pb2json.cc | 131 +++++++-
gzbridge/pb2json.hh | 67 ++++-
gzbridge/server.js | 175 +++++++++++
gzbridge/ws_server.js | 152 ----------
tools/gzcoarse.cc | 24 ++
14 files changed, 983 insertions(+), 664 deletions(-)
create mode 100755 gzbridge/server.js
delete mode 100755 gzbridge/ws_server.js
diff --git a/deploy-gzbridge-nrp.sh b/deploy-gzbridge-nrp.sh
index be72a62..42aed7e 100755
--- a/deploy-gzbridge-nrp.sh
+++ b/deploy-gzbridge-nrp.sh
@@ -4,15 +4,6 @@ DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
cd $DIR
source $HOME/.bashrc
-[ -s "$NVM_DIR/nvm.sh" ] && . "$NVM_DIR/nvm.sh"
-if ! node --version | grep -q v0.10
-then
- if ! nvm use 0.10 2>/dev/null
- then
- printf "\033[0;31mYou must use node v0.10 or use nvm and install v0.10 (ie. nvm install 0.10)\033[0m\n"
- exit 1
- fi
-fi
# Install node modules
npm install
diff --git a/gzbridge/CMakeLists.txt b/gzbridge/CMakeLists.txt
index 42e850d..db9c15c 100644
--- a/gzbridge/CMakeLists.txt
+++ b/gzbridge/CMakeLists.txt
@@ -1,6 +1,5 @@
include_directories(
include_directories(${GAZEBO_INCLUDE_DIRS})
- ${PROJECT_SOURCE_DIR}/deps/libwebsockets/lib
)
link_directories(${GAZEBO_LIBRARY_DIRS})
@@ -15,14 +14,3 @@ set (headers
pb2json.hh
GazeboInterface.hh
)
-
-# add_executable(gzbridge ${sources})
-
-# target_link_libraries(gzbridge websockets_shared ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} ${JANSSON_LIBRARIES})
-#add_dependencies(${TEST_NAME} websockets_shared)
-
-# Set test app specific defines.
-# set_property(TARGET gzbridge
-# PROPERTY COMPILE_DEFINITIONS
-# INSTALL_DATADIR="${CMAKE_INSTALL_PREFIX}/share"
-# )
diff --git a/gzbridge/ConfigLoader.hh b/gzbridge/ConfigLoader.hh
index 69a78ec..93db86e 100644
--- a/gzbridge/ConfigLoader.hh
+++ b/gzbridge/ConfigLoader.hh
@@ -1,5 +1,5 @@
-#ifndef _CONFIG_LOADER_HH_
-#define _CONFIG_LOADER_HH_
+#ifndef GZBRIDGE_CONFIG_LOADER_HH_
+#define GZBRIDGE_CONFIG_LOADER_HH_
#include <map>
#include <vector>
diff --git a/gzbridge/GZNode.cc b/gzbridge/GZNode.cc
index f2cd144..f688edc 100644
--- a/gzbridge/GZNode.cc
+++ b/gzbridge/GZNode.cc
@@ -27,20 +27,27 @@ using namespace gzweb;
/////////////////////////////////////////////////
GZNode::GZNode()
{
- isGzServerConnected = false;
if (!gazebo::transport::init()) {
return;
}
+ this->isGzServerConnected = true;
this->ConnectGzServer();
-};
+}
-GZNode::GZNode(const v8::Arguments& args) {
+GZNode::GZNode(const FunctionCallbackInfo<Value>& args) {
isGzServerConnected = false;
+ Isolate* isolate = args.GetIsolate();
+
if (args.Length() != 2) {
- ThrowException(Exception::TypeError(
- String::New("Wrong number of arguments")));
+ isolate->ThrowException(Exception::TypeError(
+ String::NewFromUtf8(
+ isolate,
+ "Wrong number of arguments"
+ )
+ )
+ );
return;
}
@@ -53,7 +60,8 @@ GZNode::GZNode(const v8::Arguments& args) {
}
this->ConnectGzServer();
-};
+
+}
void GZNode::ConnectGzServer() {
isGzServerConnected = true;
@@ -62,6 +70,7 @@ void GZNode::ConnectGzServer() {
this->gzIface = new GazeboInterface();
this->gzIface->Init();
this->gzIface->RunThread();
+
}
/////////////////////////////////////////////////
@@ -69,103 +78,80 @@ GZNode::~GZNode()
{
// Make sure to shut everything down.
gazebo::transport::fini();
-};
+}
/////////////////////////////////////////////////
void GZNode::Init(Handle<Object> exports)
{
+ Isolate* isolate = exports->GetIsolate();
// Prepare constructor template
- Local<FunctionTemplate> tpl = FunctionTemplate::New(New);
- tpl->SetClassName(String::NewSymbol("GZNode"));
+ Local<FunctionTemplate> tpl = FunctionTemplate::New(isolate, New);
+ tpl->SetClassName(String::NewFromUtf8(isolate, "GZNode"));
tpl->InstanceTemplate()->SetInternalFieldCount(1);
// Prototype
- tpl->PrototypeTemplate()->Set(String::NewSymbol("loadMaterialScripts"),
- FunctionTemplate::New(LoadMaterialScripts)->GetFunction());
-
- tpl->PrototypeTemplate()->Set(String::NewSymbol("setCallback"),
- FunctionTemplate::New(Callback)->GetFunction());
+ NODE_SET_PROTOTYPE_METHOD(tpl, "loadMaterialScripts", LoadMaterialScripts);
- tpl->PrototypeTemplate()->Set(String::NewSymbol("request"),
- FunctionTemplate::New(Request)->GetFunction());
+ NODE_SET_PROTOTYPE_METHOD(tpl, "setConnected", SetConnected);
- tpl->PrototypeTemplate()->Set(String::NewSymbol("getMessages"),
- FunctionTemplate::New(GetMessages)->GetFunction());
+ NODE_SET_PROTOTYPE_METHOD(tpl, "setPoseMsgFilterMinimumDistanceSquared", SetPoseMsgFilterMinimumDistanceSquared);
+ NODE_SET_PROTOTYPE_METHOD(tpl, "getPoseMsgFilterMinimumDistanceSquared", GetPoseMsgFilterMinimumDistanceSquared);
+ NODE_SET_PROTOTYPE_METHOD(tpl, "setPoseMsgFilterMinimumQuaternionSquared", SetPoseMsgFilterMinimumQuaternionSquared);
+ NODE_SET_PROTOTYPE_METHOD(tpl, "getPoseMsgFilterMinimumQuaternionSquared", GetPoseMsgFilterMinimumQuaternionSquared);
- tpl->PrototypeTemplate()->Set(String::NewSymbol("getPoseMsgFilterMinimumAge"),
- FunctionTemplate::New(GetPoseMsgFilterMinimumAge)->GetFunction());
+ NODE_SET_PROTOTYPE_METHOD(tpl, "getPoseMsgFilterMinimumAge",
+ GetPoseMsgFilterMinimumAge);
- tpl->PrototypeTemplate()->Set(String::NewSymbol("setPoseMsgFilterMinimumAge"),
- FunctionTemplate::New(SetPoseMsgFilterMinimumAge)->GetFunction());
+ NODE_SET_PROTOTYPE_METHOD(tpl, "setPoseMsgFilterMinimumAge",
+ SetPoseMsgFilterMinimumAge);
- tpl->PrototypeTemplate()->Set(
- String::NewSymbol("getPoseMsgFilterMinimumDistanceSquared"),
- FunctionTemplate::New(
- GetPoseMsgFilterMinimumDistanceSquared)->GetFunction());
+ NODE_SET_PROTOTYPE_METHOD(tpl, "getMessages", GetMessages);
- tpl->PrototypeTemplate()->Set(
- String::NewSymbol("setPoseMsgFilterMinimumDistanceSquared"),
- FunctionTemplate::New(
- SetPoseMsgFilterMinimumDistanceSquared)->GetFunction());
+ NODE_SET_PROTOTYPE_METHOD(tpl, "request", Request);
- tpl->PrototypeTemplate()->Set(
- String::NewSymbol("getPoseMsgFilterMinimumQuaternionSquared"),
- FunctionTemplate::New(
- GetPoseMsgFilterMinimumQuaternionSquared)->GetFunction());
+ NODE_SET_PROTOTYPE_METHOD(tpl, "getIsGzServerConnected",
+ GetIsGzServerConnected);
- tpl->PrototypeTemplate()->Set(
- String::NewSymbol("setPoseMsgFilterMinimumQuaternionSquared"),
- FunctionTemplate::New(
- SetPoseMsgFilterMinimumQuaternionSquared)->GetFunction());
+ NODE_SET_PROTOTYPE_METHOD(tpl, "getMaterialScriptsMessage",
+ GetMaterialScriptsMessage);
- tpl->PrototypeTemplate()->Set(String::NewSymbol("setConnected"),
- FunctionTemplate::New(
- SetConnected)->GetFunction());
-
- tpl->PrototypeTemplate()->Set(
- String::NewSymbol("getIsGzServerConnected"),
- FunctionTemplate::New(
- GetIsGzServerConnected)->GetFunction());
-
- tpl->PrototypeTemplate()->Set(String::NewSymbol("getMaterialScriptsMessage"),
- FunctionTemplate::New(GetMaterialScriptsMessage)->GetFunction());
-
- Persistent<Function> constructor =
- Persistent<Function>::New(tpl->GetFunction());
-
- exports->Set(String::NewSymbol("GZNode"), constructor);
+ exports->Set(String::NewFromUtf8(isolate, "GZNode"),
+ tpl->GetFunction());
}
/////////////////////////////////////////////////
-Handle<Value> GZNode::New(const Arguments& args)
+void GZNode::New(const FunctionCallbackInfo<Value>& args)
{
- HandleScope scope;
- GZNode* obj;
-
- if (args.Length() == 0) obj = new GZNode();
- else if (args.Length() == 2) obj = new GZNode(args);
-
- obj->Wrap(args.This());
-
- return args.This();
+ if (args.IsConstructCall()) {
+ // Invoked as constructor: `new MyObject(...)`
+ GZNode* gzObj = nullptr;
+ if (args.Length() == 0)
+ gzObj = new GZNode();
+ else
+ if (args.Length() == 2)
+ gzObj = new GZNode(args);
+
+ gzObj->Wrap(args.This());
+ args.GetReturnValue().Set(args.This());
+ }
}
/////////////////////////////////////////////////
-Handle<Value> GZNode::LoadMaterialScripts(const Arguments& args)
+void GZNode::LoadMaterialScripts(const FunctionCallbackInfo<Value>& args)
{
- HandleScope scope;
+ Isolate* isolate = args.GetIsolate();
if (args.Length() < 1)
{
- ThrowException(Exception::TypeError(
- String::New("Wrong number of arguments")));
- return scope.Close(Undefined());
+ isolate->ThrowException(Exception::TypeError(
+ String::NewFromUtf8(isolate, "Wrong number of arguments")));
+ return;
}
if (!args[0]->IsString())
{
- ThrowException(Exception::TypeError(
- String::New("Wrong argument type. String expected.")));
- return scope.Close(Undefined());
+ isolate->ThrowException(Exception::TypeError(
+ String::NewFromUtf8(isolate, "Wrong argument type. String expected.")));
+ return;
}
GZNode* obj = ObjectWrap::Unwrap<GZNode>(args.This());
@@ -173,187 +159,170 @@ Handle<Value> GZNode::LoadMaterialScripts(const Arguments& args)
String::Utf8Value path(args[0]->ToString());
obj->gzIface->LoadMaterialScripts(std::string(*path));
- return scope.Close(Undefined());
+ return;
}
-Handle<Value> GZNode::GetMaterialScriptsMessage(const Arguments& args)
+/////////////////////////////////////////////////
+void GZNode::SetConnected(const FunctionCallbackInfo<Value>& args)
{
- HandleScope scope;
-
- if (args.Length() < 1)
- {
- ThrowException(Exception::TypeError(
- String::New("Wrong number of arguments")));
- return scope.Close(Undefined());
- }
-
- if (!args[0]->IsString())
- {
- ThrowException(Exception::TypeError(
- String::New("Wrong argument type. String expected.")));
- return scope.Close(Undefined());
- }
+ GZNode *obj = ObjectWrap::Unwrap<GZNode>(args.This());
+ bool value = args[0]->BooleanValue();
+ obj->gzIface->SetConnected(value);
- String::Utf8Value path(args[0]->ToString());
+ return;
+}
- OgreMaterialParser materialParser;
- materialParser.Load(std::string(*path));
- std::string topic = "~/material";
- std::string materialJson = materialParser.GetMaterialAsJson();
- std::string msg;
- msg += "{\"op\":\"publish\",\"topic\":\"" + topic + "\", \"msg\":";
- msg += materialJson;
- msg += "}";
+/////////////////////////////////////////////////
+void GZNode::GetIsGzServerConnected(const FunctionCallbackInfo<Value>& args)
+{
+ GZNode *obj = ObjectWrap::Unwrap<GZNode>(args.This());
+ bool value = obj->isGzServerConnected;
- return scope.Close(String::New(msg.c_str()));
+ args.GetReturnValue().Set(value);
}
/////////////////////////////////////////////////
-Handle<Value> GZNode::Request(const Arguments& args)
+void GZNode::GetMaterialScriptsMessage(const FunctionCallbackInfo<Value>& args)
{
- HandleScope scope;
+ Isolate* isolate = args.GetIsolate();
if (args.Length() < 1)
{
- ThrowException(Exception::TypeError(
- String::New("Wrong number of arguments")));
- return scope.Close(Undefined());
+ isolate->ThrowException(Exception::TypeError(
+ String::NewFromUtf8(isolate, "Wrong number of arguments")));
+ return;
}
if (!args[0]->IsString())
{
- ThrowException(Exception::TypeError(
- String::New("Wrong argument type. String expected.")));
- return scope.Close(Undefined());
+ isolate->ThrowException(Exception::TypeError(
+ String::NewFromUtf8(isolate, "Wrong argument type. String expected.")));
+ return;
}
- GZNode* obj = ObjectWrap::Unwrap<GZNode>(args.This());
-
- String::Utf8Value request(args[0]->ToString());
- obj->gzIface->PushRequest(std::string(*request));
-
- return scope.Close(Undefined());
-}
-
-/////////////////////////////////////////////////
-Handle<Value> GZNode::Callback(const Arguments& args) {
- HandleScope scope;
+ String::Utf8Value path(args[0]->ToString());
- v8::Local<v8::Function> cb = Local<Function>::Cast(args[0]);
- const unsigned argc = 1;
- Local<Value> argv[argc] = { Local<Value>::New(String::New("hello world")) };
- cb->Call(Context::GetCurrent()->Global(), argc, argv);
+ OgreMaterialParser materialParser;
+ materialParser.Load(std::string(*path));
+ std::string topic = "~/material";
+ std::string materialJson = materialParser.GetMaterialAsJson();
+ std::string msg;
+ msg += "{\"op\":\"publish\",\"topic\":\"" + topic + "\", \"msg\":";
+ msg += materialJson;
+ msg += "}";
- return scope.Close(Undefined());
+ args.GetReturnValue().Set(String::NewFromUtf8(isolate, msg.c_str()));
}
/////////////////////////////////////////////////
-Handle<v8::Value> GZNode::SetPoseMsgFilterMinimumAge(const v8::Arguments& args)
+void GZNode::SetPoseMsgFilterMinimumDistanceSquared(const
+ FunctionCallbackInfo<Value>& args)
{
- HandleScope scope;
+ GZNode *obj = ObjectWrap::Unwrap<GZNode>(args.This());
- GZNode* obj = ObjectWrap::Unwrap<GZNode>(args.This());
Local<Number> v = Local<Number>::Cast(args[0]);
double value = v->Value();
- obj->gzIface->SetPoseFilterMinimumMsgAge(value);
+ obj->gzIface->SetPoseFilterMinimumDistanceSquared(value);
- return scope.Close(Undefined());
+ return;
}
/////////////////////////////////////////////////
-Handle<v8::Value> GZNode::GetPoseMsgFilterMinimumAge(const \
- v8::Arguments& args)
+void GZNode::GetPoseMsgFilterMinimumDistanceSquared(const
+ FunctionCallbackInfo<Value>& args)
{
- HandleScope scope;
- GZNode* obj = ObjectWrap::Unwrap<GZNode>(args.This());
- double value = obj->gzIface->GetPoseFilterMinimumMsgAge();
- return scope.Close(Number::New(value));
+ Isolate* isolate = args.GetIsolate();
+ GZNode *obj = ObjectWrap::Unwrap<GZNode>(args.This());
+ double value = obj->gzIface->GetPoseFilterMinimumDistanceSquared();
+ args.GetReturnValue().Set(Number::New(isolate ,value));
}
-/////////////////////////////////////////////////
-Handle<v8::Value> GZNode::SetPoseMsgFilterMinimumDistanceSquared(const
- v8::Arguments& args)
+/////////////////////////////////////////////////////
+void GZNode::SetPoseMsgFilterMinimumQuaternionSquared(const
+ FunctionCallbackInfo<Value>& args)
{
- HandleScope scope;
-
- GZNode* obj = ObjectWrap::Unwrap<GZNode>(args.This());
+ GZNode *obj = ObjectWrap::Unwrap<GZNode>(args.This());
Local<Number> v = Local<Number>::Cast(args[0]);
double value = v->Value();
- obj->gzIface->SetPoseFilterMinimumDistanceSquared(value);
+ obj->gzIface->SetPoseFilterMinimumQuaternionSquared(value);
- return scope.Close(Undefined());
+ return;
}
/////////////////////////////////////////////////
-Handle<v8::Value> GZNode::GetPoseMsgFilterMinimumDistanceSquared(const
- v8::Arguments& args)
+void GZNode::GetPoseMsgFilterMinimumQuaternionSquared(const
+ FunctionCallbackInfo<Value>& args)
{
- HandleScope scope;
- GZNode* obj = ObjectWrap::Unwrap<GZNode>(args.This());
- double value = obj->gzIface->GetPoseFilterMinimumDistanceSquared();
- return scope.Close(Number::New(value));
+ Isolate* isolate = args.GetIsolate();
+ GZNode *obj = ObjectWrap::Unwrap<GZNode>(args.This());
+ double value = obj->gzIface->GetPoseFilterMinimumQuaternionSquared();
+ args.GetReturnValue().Set(Number::New(isolate ,value));
}
/////////////////////////////////////////////////
-Handle<v8::Value> GZNode::SetPoseMsgFilterMinimumQuaternionSquared(const
- v8::Arguments& args)
+void GZNode::GetMessages(const FunctionCallbackInfo<Value>& args)
{
- HandleScope scope;
+ Isolate* isolate = args.GetIsolate();
GZNode* obj = ObjectWrap::Unwrap<GZNode>(args.This());
- Local<Number> v = Local<Number>::Cast(args[0]);
- double value = v->Value();
- obj->gzIface->SetPoseFilterMinimumQuaternionSquared(value);
- return scope.Close(Undefined());
+ std::vector<std::string> msgs = obj->gzIface->PopOutgoingMessages();
+ // args.GetReturnValue().Set(Number::New(isolate ,msgs.size()));
+ Local<Array> arguments = Array::New(isolate, msgs.size());
+ for (unsigned int i = 0; i < msgs.size(); ++i) {
+ arguments->Set(i ,String::NewFromUtf8(isolate, msgs[i].c_str()));
+ }
+
+ args.GetReturnValue().Set(arguments);
}
-/////////////////////////////////////////////////
-Handle<v8::Value> GZNode::SetConnected(const v8::Arguments& args)
+
+////////////////////////////////////////////////
+void GZNode::Request(const FunctionCallbackInfo<Value>& args)
{
- HandleScope scope;
+ Isolate* isolate = args.GetIsolate();
- GZNode *obj = ObjectWrap::Unwrap<GZNode>(args.This());
- bool value = args[0]->BooleanValue();
- obj->gzIface->SetConnected(value);
+ if (args.Length() < 1)
+ {
+ isolate->ThrowException(Exception::TypeError(
+ String::NewFromUtf8(isolate, "Wrong number of arguments")));
+ return;
+ }
- return scope.Close(Undefined());
-}
+ if (!args[0]->IsString())
+ {
+ isolate->ThrowException(Exception::TypeError(
+ String::NewFromUtf8(isolate, "Wrong argument type. String expected.")));
+ return;
+ }
-Handle<v8::Value> GZNode::GetIsGzServerConnected(const v8::Arguments& args)
-{
- HandleScope scope;
+ GZNode* obj = ObjectWrap::Unwrap<GZNode>(args.This());
- GZNode *obj = ObjectWrap::Unwrap<GZNode>(args.This());
- bool value = obj->isGzServerConnected;
+ String::Utf8Value request(args[0]->ToString());
+ obj->gzIface->PushRequest(std::string(*request));
- return scope.Close(Boolean::New(value));
+ return;
}
/////////////////////////////////////////////////
-Handle<v8::Value> GZNode::GetPoseMsgFilterMinimumQuaternionSquared(const
- v8::Arguments& args)
+void GZNode::SetPoseMsgFilterMinimumAge(const
+ FunctionCallbackInfo<Value>& args)
{
- HandleScope scope;
GZNode* obj = ObjectWrap::Unwrap<GZNode>(args.This());
- double value = obj->gzIface->GetPoseFilterMinimumQuaternionSquared();
- return scope.Close(Number::New(value));
+ Local<Number> v = Local<Number>::Cast(args[0]);
+ double value = v->Value();
+ obj->gzIface->SetPoseFilterMinimumMsgAge(value);
+
+ return;
}
/////////////////////////////////////////////////
-Handle<Value> GZNode::GetMessages(const Arguments& args)
+void GZNode::GetPoseMsgFilterMinimumAge(const
+ FunctionCallbackInfo<Value>& args)
{
- HandleScope scope;
-
GZNode* obj = ObjectWrap::Unwrap<GZNode>(args.This());
-
- std::vector<std::string> msgs = obj->gzIface->PopOutgoingMessages();
- Local<Array> arguments = Array::New(msgs.size());
- for (unsigned int i = 0; i < msgs.size(); ++i) {
- arguments->Set(i ,String::New(msgs[i].c_str()));
- }
-
- return scope.Close(arguments);
+ double value = obj->gzIface->GetPoseFilterMinimumMsgAge();
+ args.GetReturnValue().Set(value);
}
/////////////////////////////////////////////////
diff --git a/gzbridge/GZNode.hh b/gzbridge/GZNode.hh
index 24944e0..5d3f041 100644
--- a/gzbridge/GZNode.hh
+++ b/gzbridge/GZNode.hh
@@ -15,15 +15,20 @@
*
*/
-#ifndef _GZNODE_HH_
-#define _GZNODE_HH_
+#ifndef GZBRIDGE_GZNODE_HH_
+#define GZBRIDGE_GZNODE_HH_
#include <node.h>
-#include <string>
-#include <vector>
+#include <node_object_wrap.h>
namespace gzweb
{
+ using v8::FunctionCallbackInfo;
+ using v8::Value;
+ using v8::FunctionTemplate;
+ using v8::Object;
+ using v8::Persistent;
+
class GazeboInterface;
class GZNode : public node::ObjectWrap
@@ -32,53 +37,51 @@ namespace gzweb
private: GZNode();
- private: GZNode(const v8::Arguments& args);
-
- private: void ConnectGzServer();
+ private: GZNode(const FunctionCallbackInfo<Value>& args);
private: ~GZNode();
- private: static v8::Handle<v8::Value> New(const v8::Arguments& args);
+ private: static void New(const FunctionCallbackInfo<Value>& args);
- private: static v8::Handle<v8::Value> Callback(const v8::Arguments& args);
+ private: static void LoadMaterialScripts(
+ const FunctionCallbackInfo<Value>& args);
- private: static v8::Handle<v8::Value> Request(const v8::Arguments& args);
+ private: static void SetConnected(
+ const FunctionCallbackInfo<Value>& args);
- private: static v8::Handle<v8::Value> SetPoseMsgFilterMinimumAge(const
- v8::Arguments& args);
+ private: static void GetIsGzServerConnected(
+ const FunctionCallbackInfo<Value>& args);
- private: static v8::Handle<v8::Value> GetPoseMsgFilterMinimumAge(const
- v8::Arguments& args);
+ private: static void GetMaterialScriptsMessage(
+ const FunctionCallbackInfo<Value>& args);
- private: static v8::Handle<v8::Value>
- SetPoseMsgFilterMinimumDistanceSquared(const v8::Arguments& args);
+ private: static void SetPoseMsgFilterMinimumDistanceSquared(
+ const FunctionCallbackInfo<Value>& args);
- private: static v8::Handle<v8::Value>
- GetPoseMsgFilterMinimumDistanceSquared(const v8::Arguments& args);
+ private: static void GetPoseMsgFilterMinimumDistanceSquared(
+ const FunctionCallbackInfo<Value>& args);
- private: static v8::Handle<v8::Value>
- SetPoseMsgFilterMinimumQuaternionSquared(const v8::Arguments& args);
+ private: static void SetPoseMsgFilterMinimumQuaternionSquared(
+ const FunctionCallbackInfo<Value>& args);
- private: static v8::Handle<v8::Value>
- SetConnected(const v8::Arguments& args);
+ private: static void GetPoseMsgFilterMinimumQuaternionSquared(
+ const FunctionCallbackInfo<Value>& args);
- private: static v8::Handle<v8::Value>
- GetPoseMsgFilterMinimumQuaternionSquared(const v8::Arguments& args);
+ private: static void GetMessages(const FunctionCallbackInfo<Value>& args);
- private: static v8::Handle<v8::Value>
- GetMessages(const v8::Arguments& args);
+ private: static void Request(const FunctionCallbackInfo<Value>& args);
- private: static v8::Handle<v8::Value>
- LoadMaterialScripts(const v8::Arguments& args);
+ private: static void SetPoseMsgFilterMinimumAge(
+ const FunctionCallbackInfo<Value>& args);
- private: static v8::Handle<v8::Value>
- GetIsGzServerConnected(const v8::Arguments& args);
+ private: static void GetPoseMsgFilterMinimumAge(
+ const FunctionCallbackInfo<Value>& args);
+
+ private: void ConnectGzServer();
- private: static v8::Handle<v8::Value>
- GetMaterialScriptsMessage(const v8::Arguments& args);
+ private: GazeboInterface* gzIface = nullptr;
- private: GazeboInterface* gzIface;
- private: bool isGzServerConnected;
+ private: bool isGzServerConnected = false;
};
}
diff --git a/gzbridge/GazeboInterface.cc b/gzbridge/GazeboInterface.cc
index 2780e59..7851c35 100644
--- a/gzbridge/GazeboInterface.cc
+++ b/gzbridge/GazeboInterface.cc
@@ -15,7 +15,7 @@
*
*/
-#include <boost/thread.hpp>
+#include <gazebo/gazebo_config.h>
#include "pb2json.hh"
#include "OgreMaterialParser.hh"
@@ -25,18 +25,9 @@
using namespace gzweb;
-std::vector<std::string> GazeboInterface::incoming;
-std::vector<std::string> GazeboInterface::outgoing;
-
-boost::recursive_mutex incomingMutex;
-boost::recursive_mutex outgoingMutex;
-
/////////////////////////////////////////////////
GazeboInterface::GazeboInterface()
{
-// this->socketServer = _server;
- this->receiveMutex = new boost::recursive_mutex();
- this->serviceMutex = new boost::recursive_mutex();
this->stop = false;
// Create our node for communication
@@ -71,6 +62,7 @@ GazeboInterface::GazeboInterface()
this->roadTopic = "~/roads";
this->heightmapService = "~/heightmap_data";
this->deleteTopic = "~/entity_delete";
+ this->playbackControlTopic = "~/playback_control";
// material topic
this->materialTopic = "~/material";
@@ -149,20 +141,22 @@ GazeboInterface::GazeboInterface()
this->node->Advertise<gazebo::msgs::WorldControl>(
this->worldControlTopic);
+ // For controlling playback
+ this->playbackControlPub =
+ this->node->Advertise<gazebo::msgs::LogPlaybackControl>(
+ this->playbackControlTopic);
+
+
this->responseSub = this->node->Subscribe("~/response",
&GazeboInterface::OnResponse, this);
this->materialParser = new OgreMaterialParser();
- this->lastStatsTime = gazebo::common::Time::Zero;
this->lastPausedState = true;
// message filtering apparatus
this->minimumDistanceSquared = 0.0001;
this->minimumQuaternionSquared = 0.0001;
- this->minimumDistanceSquaredTooEarly = 0.01;
- this->minimumQuaternionSquaredTooEarly = 0.01;
-
this->minimumMsgAge = 0.03;
this->skippedMsgCount = 0;
this->messageWindowSize = 10000;
@@ -170,14 +164,7 @@ GazeboInterface::GazeboInterface()
this->isConnected = false;
- // initialize thread variables
- this->runThread = NULL;
- this->serviceThread = NULL;
- this->connectionCondition = new boost::condition_variable();
- this->connectionMutex = new boost::mutex();
- this->serviceCondition = new boost::condition_variable_any();
this->receivedMessage = false;
- this->receiveCondition = new boost::condition_variable_any();
}
/////////////////////////////////////////////////
@@ -230,22 +217,11 @@ GazeboInterface::~GazeboInterface()
this->node.reset();
if (this->runThread)
- {
this->runThread->join();
- delete this->runThread;
- }
if (this->serviceThread)
- {
this->serviceThread->join();
- delete this->serviceThread;
- }
- delete this->receiveMutex;
- delete this->serviceMutex;
- delete this->connectionCondition;
- delete this->connectionMutex;
- delete this->serviceCondition;
- delete this->receiveCondition;
+ delete this->materialParser;
}
/////////////////////////////////////////////////
@@ -257,10 +233,10 @@ void GazeboInterface::Init()
/////////////////////////////////////////////////
void GazeboInterface::RunThread()
{
- this->runThread = new boost::thread(boost::bind(&GazeboInterface::Run, this));
- this->serviceThread = new boost::thread(
- boost::bind(&GazeboInterface::RunService, this));
-
+ this->runThread.reset(
+ new std::thread(std::bind(&GazeboInterface::Run, this)));
+ this->serviceThread.reset(new std::thread(
+ std::bind(&GazeboInterface::RunService, this)));
}
/////////////////////////////////////////////////
@@ -294,28 +270,8 @@ void GazeboInterface::Fini()
/////////////////////////////////////////////////
void GazeboInterface::ProcessMessages()
{
- static RequestMsgs_L::iterator rIter;
- static SceneMsgs_L::iterator sIter;
- static PhysicsMsgs_L::iterator physicsIter;
- static WorldStatsMsgs_L::iterator wIter;
- static ModelMsgs_L::iterator modelIter;
- static VisualMsgs_L::iterator visualIter;
- static LightMsgs_L::iterator lightIter;
- static PoseMsgs_L::iterator pIter;
- // static SkeletonPoseMsgs_L::iterator spIter;
- static JointMsgs_L::iterator jointIter;
- static SensorMsgs_L::iterator sensorIter;
-
-#ifdef GAZEBO_HBP_SUPPORT_JOINT_STATE_MESSAGES
- static JointStateMsgs_L::iterator jointStateIter;
-#endif
-
-#ifdef GAZEBO_HBP_SUPPORT_MUSCLE_VISUALIZATION_MESSAGES
- static MuscleVisualizationMsgs_L::iterator muscleVisualizationIter;
-#endif
-
{
- boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
+ std::lock_guard<std::recursive_mutex> lock(this->receiveMutex);
// Process incoming messages.
std::vector<std::string> msgs = this->PopIncomingMessages();
@@ -348,7 +304,8 @@ void GazeboInterface::ProcessMessages()
{
gazebo::msgs::Request *requestPhysicsMsg;
requestPhysicsMsg = gazebo::msgs::CreateRequest("physics_info", "");
- if (this->requests.find(requestPhysicsMsg->id()) != this->requests.end())
+ if (this->requests.find(requestPhysicsMsg->id()) !=
+ this->requests.end())
requests.erase(requestPhysicsMsg->id());
this->requests[requestPhysicsMsg->id()] = requestPhysicsMsg;
this->requestPub->Publish(*requestPhysicsMsg);
@@ -361,36 +318,115 @@ void GazeboInterface::ProcessMessages()
}
else if (topic == this->modelModifyTopic)
{
- std::string name = get_value(msg, "msg:name");
- int id = atoi(get_value(msg, "msg:id").c_str());
+ JsonObj jsonObj(msg);
+ JsonObj msgObj = jsonObj.Object("msg");
+ if (!msgObj)
+ {
+ std::cerr << "No msg key in json object" << std::endl;
+ continue;
+ }
+ std::string name = msgObj.Object("name").String();
if (name == "")
continue;
- ignition::math::Vector3d pos(
- atof(get_value(msg, "msg:position:x").c_str()),
- atof(get_value(msg, "msg:position:y").c_str()),
- atof(get_value(msg, "msg:position:z").c_str()));
- ignition::math::Vector3d scale(
- atof(get_value(msg, "msg:scale:x").c_str()),
- atof(get_value(msg, "msg:scale:y").c_str()),
- atof(get_value(msg, "msg:scale:z").c_str()));
- ignition::math::Quaterniond quat(
- atof(get_value(msg, "msg:orientation:w").c_str()),
- atof(get_value(msg, "msg:orientation:x").c_str()),
- atof(get_value(msg, "msg:orientation:y").c_str()),
- atof(get_value(msg, "msg:orientation:z").c_str()));
+ unsigned int id =
+ static_cast<unsigned int>(msgObj.Object("id").Number());
+
+ JsonObj posObj = msgObj.Object("position");
+ if (!posObj)
+ {
+ std::cerr << "model pose msg is missing 'position' field"
+ << std::endl;
+ continue;
+ }
+
+ double posX = posObj.Object("x").Number();
+ double posY = posObj.Object("y").Number();
+ double posZ = posObj.Object("z").Number();
+ JsonObj quatObj = msgObj.Object("orientation");
+ if (!quatObj)
+ {
+ std::cerr << "model pose msg is missing 'orientation' field"
+ << std::endl;
+ continue;
+ }
+ double quatW = quatObj.Object("w").Number();
+ double quatX = quatObj.Object("x").Number();
+ double quatY = quatObj.Object("y").Number();
+ double quatZ = quatObj.Object("z").Number();
+
+
+ JsonObj scaleObj = msgObj.Object("scale");
+ if (!scaleObj)
+ {
+ std::cerr << "model pose msg is missing 'scale' field"
+ << std::endl;
+ continue;
+ }
+ double scaleX = scaleObj.Object("x").Number();
+ double scaleY = scaleObj.Object("y").Number();
+ double scaleZ = scaleObj.Object("z").Number();
+
+ ignition::math::Vector3d pos(posX, posY, posZ);
+ ignition::math::Quaterniond quat(quatW, quatX, quatY, quatZ);
+
ignition::math::Pose3d pose(pos, quat);
+ ignition::math::Vector3d scale(scaleX, scaleY, scaleZ);
+
gazebo::msgs::Model modelMsg;
modelMsg.set_id(id);
modelMsg.set_name(name);
gazebo::msgs::Set(modelMsg.mutable_pose(), pose);
gazebo::msgs::Set(modelMsg.mutable_scale(), scale);
+ JsonObj linksObj = msgObj.Object("link");
+ if (linksObj)
+ {
+ for (unsigned int i = 0; i < linksObj.ArraySize(); ++i)
+ {
+ JsonObj linkObj = linksObj.ArrayObject(i);
+ std::string linkName = linkObj.Object("name").String();
+ unsigned int linkId =
+ static_cast<unsigned int>(linkObj.Object("id").Number());
+ JsonObj linkPosObj = linkObj.Object("position");
+ if (!linkPosObj)
+ {
+ std::cerr << "link pose msg is missing 'position' field"
+ << std::endl;
+ continue;
+ }
+ double linkPosX = linkPosObj.Object("x").Number();
+ double linkPosY = linkPosObj.Object("y").Number();
+ double linkPosZ = linkPosObj.Object("z").Number();
+ JsonObj linkQuatObj = linkObj.Object("orientation");
+ if (!linkQuatObj)
+ {
+ std::cerr << "link pose msg is missing 'orientation' field"
+ << std::endl;
+ continue;
+ }
+ double linkQuatW = linkQuatObj.Object("w").Number();
+ double linkQuatX = linkQuatObj.Object("x").Number();
+ double linkQuatY = linkQuatObj.Object("y").Number();
+ double linkQuatZ = linkQuatObj.Object("z").Number();
+
+ ignition::math::Vector3d linkPos(linkPosX, linkPosY, linkPosZ);
+ ignition::math::Quaterniond linkQuat(
+ linkQuatW, linkQuatX, linkQuatY, linkQuatZ);
+ ignition::math::Pose3d linkPose(linkPos, linkQuat);
+
+ gazebo::msgs::Link *linkMsg = modelMsg.add_link();
+ linkMsg->set_id(linkId);
+ linkMsg->set_name(linkName);
+ gazebo::msgs::Set(linkMsg->mutable_pose(), linkPose);
+ }
+ }
this->modelPub->Publish(modelMsg);
}
- else if (topic == this->lightFactoryTopic || topic == this->lightModifyTopic)
+ else if (topic == this->lightFactoryTopic ||
+ topic == this->lightModifyTopic)
{
std::string name = get_value(msg, "msg:name");
std::string type = get_value(msg, "msg:type");
@@ -424,13 +460,20 @@ void GazeboInterface::ProcessMessages()
atof(get_value(msg, "msg:direction:z").c_str()));
gazebo::msgs::Set(lightMsg.mutable_direction(), direction);
+#if GAZEBO_MAJOR_VERSION >= 9
+ ignition::math::Color diffuse(
+#else
gazebo::common::Color diffuse(
+#endif
atof(get_value(msg, "msg:diffuse:r").c_str()),
atof(get_value(msg, "msg:diffuse:g").c_str()),
atof(get_value(msg, "msg:diffuse:b").c_str()), 1);
gazebo::msgs::Set(lightMsg.mutable_diffuse(), diffuse);
-
+#if GAZEBO_MAJOR_VERSION >= 9
+ ignition::math::Color specular(
+#else
gazebo::common::Color specular(
+#endif
atof(get_value(msg, "msg:specular:r").c_str()),
atof(get_value(msg, "msg:specular:g").c_str()),
atof(get_value(msg, "msg:specular:b").c_str()), 1);
@@ -464,11 +507,20 @@ void GazeboInterface::ProcessMessages()
gazebo::msgs::Set(lightMsg.mutable_direction(),
ignition::math::Vector3d(0,0,-1));
}
-
gazebo::msgs::Set(lightMsg.mutable_diffuse(),
+
+#if GAZEBO_MAJOR_VERSION >= 9
+ ignition::math::Color(0.5, 0.5, 0.5, 1));
+#else
gazebo::common::Color(0.5, 0.5, 0.5, 1));
+#endif
gazebo::msgs::Set(lightMsg.mutable_specular(),
+
+#if GAZEBO_MAJOR_VERSION >= 9
+ ignition::math::Color(0.1, 0.1, 0.1, 1));
+#else
gazebo::common::Color(0.1, 0.1, 0.1, 1));
+#endif
lightMsg.set_attenuation_constant(0.5);
lightMsg.set_attenuation_linear(0.01);
lightMsg.set_attenuation_quadratic(0.001);
@@ -501,7 +553,8 @@ void GazeboInterface::ProcessMessages()
linkName = linkName.substr(index+1);
linkMsg->set_name(linkName);
- std::string self_collideStr = get_value(msg, "msg:link:self_collide").c_str();
+ std::string self_collideStr =
+ get_value(msg, "msg:link:self_collide").c_str();
bool self_collide = false;
if (self_collideStr == "1")
{
@@ -509,7 +562,8 @@ void GazeboInterface::ProcessMessages()
}
linkMsg->set_self_collide(self_collide);
- std::string gravityStr = get_value(msg, "msg:link:gravity").c_str();
+ std::string gravityStr =
+ get_value(msg, "msg:link:gravity").c_str();
bool gravity = false;
if (gravityStr == "1")
{
@@ -517,7 +571,8 @@ void GazeboInterface::ProcessMessages()
}
linkMsg->set_gravity(gravity);
- std::string kinematicStr = get_value(msg, "msg:link:kinematic").c_str();
+ std::string kinematicStr =
+ get_value(msg, "msg:link:kinematic").c_str();
bool kinematic = false;
if (kinematicStr == "1")
{
@@ -571,8 +626,10 @@ void GazeboInterface::ProcessMessages()
newModelStr << "<sdf version ='" << SDF_VERSION << "'>"
<< "<model name='" << name << "'>"
- << "<pose>" << pos.X() << " " << pos.Y() << " " << pos.Z() << " "
- << rpy.X() << " " << rpy.Y() << " " << rpy.Z() << "</pose>"
+ << "<pose>" << pos.X() << " " << pos.Y() << " " << pos.Z()
+ << " "
+ << rpy.X() << " " << rpy.Y() << " " << rpy.Z()
+ << "</pose>"
<< "<link name ='link'>"
<< "<inertial><mass>1.0</mass></inertial>"
<< "<collision name ='collision'>"
@@ -596,6 +653,11 @@ void GazeboInterface::ProcessMessages()
<< "</model>"
<< "</sdf>";
}
+ else if (type == "sdf")
+ {
+ std::string sdfStr = get_value(msg, "msg:sdf");
+ newModelStr << sdfStr;
+ }
else
{
newModelStr << "<sdf version ='" << SDF_VERSION << "'>"
@@ -611,8 +673,16 @@ void GazeboInterface::ProcessMessages()
}
// Spawn the model in the physics server
- factoryMsg.set_sdf(newModelStr.str());
- this->factoryPub->Publish(factoryMsg);
+ if (!newModelStr.str().empty())
+ {
+ factoryMsg.set_sdf(newModelStr.str());
+ this->factoryPub->Publish(factoryMsg);
+ }
+ else
+ {
+ std::cerr << "Empty model SDF string when publishing to ~/factory"
+ << std::endl;
+ }
}
else if (topic == this->worldControlTopic)
{
@@ -635,7 +705,6 @@ void GazeboInterface::ProcessMessages()
else if (reset == "world")
{
worldControlMsg.mutable_reset()->set_all(true);
-
}
}
if (!pause.empty() || !reset.empty())
@@ -654,8 +723,53 @@ void GazeboInterface::ProcessMessages()
}
else if (topic == this->deleteTopic)
{
- std::string name = get_value(msg, "msg:name");
- gazebo::transport::requestNoReply(this->node, "entity_delete", name);
+ std::string name = get_value(msg, "msg:name");
+ gazebo::transport::requestNoReply(this->node, "entity_delete", name);
+ }
+ else if (topic == this->playbackControlTopic)
+ {
+ gazebo::msgs::LogPlaybackControl playbackControlMsg;
+ std::string pause = get_value(msg, "msg:pause");
+ std::string multiStep = get_value(msg, "msg:multi_step");
+ std::string rewind = get_value(msg, "msg:rewind");
+ std::string forward = get_value(msg, "msg:forward");
+ std::string seekSec = get_value(msg, "msg:seek:sec");
+ std::string seekNSec = get_value(msg, "msg:seek:nsec");
+ if (!pause.empty())
+ {
+ int pauseValue = atoi(pause.c_str());
+ playbackControlMsg.set_pause(pauseValue);
+ }
+ if (!multiStep.empty())
+ {
+ int multiStepValue = atoi(multiStep.c_str());
+ playbackControlMsg.set_multi_step(multiStepValue);
+ }
+ if (!rewind.empty())
+ {
+ int rewindValue = atoi(rewind.c_str());
+ playbackControlMsg.set_rewind(rewindValue);
+ }
+ if (!forward.empty())
+ {
+ int forwardValue = atoi(forward.c_str());
+ playbackControlMsg.set_forward(forwardValue);
+ }
+ if (!seekSec.empty() && !seekNSec.empty())
+ {
+ auto seek = playbackControlMsg.mutable_seek();
+ seek->set_sec(atof(seekSec.c_str()));
+ seek->set_nsec(atof(seekNSec.c_str()));
+ }
+ this->playbackControlPub->Publish(playbackControlMsg);
+ }
+ else if (topic == this->statsTopic)
+ {
+ // simulate latching stats topic
+ if (this->statsMsgs.empty())
+ {
+ this->statsMsgs.push_back(this->statsMsg);
+ }
}
}
else
@@ -664,18 +778,18 @@ void GazeboInterface::ProcessMessages()
std::string service = get_value(msg.c_str(), "service");
if (!service.empty())
{
- boost::recursive_mutex::scoped_lock lock(*this->serviceMutex);
+ std::unique_lock<std::recursive_mutex> lock(this->serviceMutex);
this->serviceRequests.push_back(msg);
lock.unlock();
- this->serviceCondition->notify_one();
+ this->serviceCondition.notify_one();
}
}
}
- std::string msg = "";
+ std::string msg;
// Forward the scene messages.
- for (sIter = this->sceneMsgs.begin(); sIter != this->sceneMsgs.end();
+ for (auto sIter = this->sceneMsgs.begin(); sIter != this->sceneMsgs.end();
++sIter)
{
msg = this->PackOutgoingTopicMsg(this->sceneTopic,
@@ -685,7 +799,7 @@ void GazeboInterface::ProcessMessages()
this->sceneMsgs.clear();
// Forward the physics messages.
- for (physicsIter = this->physicsMsgs.begin();
+ for (auto physicsIter = this->physicsMsgs.begin();
physicsIter != this->physicsMsgs.end(); ++physicsIter)
{
msg = this->PackOutgoingTopicMsg(this->physicsTopic,
@@ -695,7 +809,7 @@ void GazeboInterface::ProcessMessages()
this->physicsMsgs.clear();
// Forward the model messages.
- for (modelIter = this->modelMsgs.begin();
+ for (auto modelIter = this->modelMsgs.begin();
modelIter != this->modelMsgs.end(); ++modelIter)
{
msg = this->PackOutgoingTopicMsg(this->modelTopic,
@@ -705,7 +819,7 @@ void GazeboInterface::ProcessMessages()
this->modelMsgs.clear();
// Forward the sensor messages.
- for (sensorIter = this->sensorMsgs.begin();
+ for (auto sensorIter = this->sensorMsgs.begin();
sensorIter != this->sensorMsgs.end(); ++sensorIter)
{
msg = this->PackOutgoingTopicMsg(this->sensorTopic,
@@ -715,7 +829,7 @@ void GazeboInterface::ProcessMessages()
this->sensorMsgs.clear();
// Forward the light factory messages.
- for (lightIter = this->lightFactoryMsgs.begin();
+ for (auto lightIter = this->lightFactoryMsgs.begin();
lightIter != this->lightFactoryMsgs.end(); ++lightIter)
{
msg = this->PackOutgoingTopicMsg(this->lightFactoryTopic,
@@ -725,7 +839,7 @@ void GazeboInterface::ProcessMessages()
this->lightFactoryMsgs.clear();
// Forward the light modify messages.
- for (lightIter = this->lightModifyMsgs.begin();
+ for (auto lightIter = this->lightModifyMsgs.begin();
lightIter != this->lightModifyMsgs.end(); ++lightIter)
{
msg = this->PackOutgoingTopicMsg(this->lightModifyTopic,
@@ -735,7 +849,7 @@ void GazeboInterface::ProcessMessages()
this->lightModifyMsgs.clear();
// Forward the visual messages.
- for (visualIter = this->visualMsgs.begin();
+ for (auto visualIter = this->visualMsgs.begin();
visualIter != this->visualMsgs.end(); ++visualIter)
{
msg = this->PackOutgoingTopicMsg(this->visualTopic,
@@ -745,7 +859,7 @@ void GazeboInterface::ProcessMessages()
this->visualMsgs.clear();
// Forward the joint messages.
- for (jointIter = this->jointMsgs.begin();
+ for (auto jointIter = this->jointMsgs.begin();
jointIter != this->jointMsgs.end(); ++jointIter)
{
msg = this->PackOutgoingTopicMsg(this->jointTopic,
@@ -756,7 +870,7 @@ void GazeboInterface::ProcessMessages()
#ifdef GAZEBO_HBP_SUPPORT_JOINT_STATE_MESSAGES
// Forward joint state messages.
- for (jointStateIter = this->jointStateMsgs.begin();
+ for (auto jointStateIter = this->jointStateMsgs.begin();
jointStateIter != this->jointStateMsgs.end(); ++jointStateIter)
{
msg = this->PackOutgoingTopicMsg(this->jointStatesTopic,
@@ -768,7 +882,7 @@ void GazeboInterface::ProcessMessages()
#ifdef GAZEBO_HBP_SUPPORT_MUSCLE_VISUALIZATION_MESSAGES
// Forward muscle visualization messages.
- for (muscleVisualizationIter = this->muscleVisualizationMsgs.begin();
+ for (auto muscleVisualizationIter = this->muscleVisualizationMsgs.begin();
muscleVisualizationIter != this->muscleVisualizationMsgs.end(); ++muscleVisualizationIter)
{
msg = this->PackOutgoingTopicMsg(this->muscleVisualizationTopic,
@@ -779,8 +893,8 @@ void GazeboInterface::ProcessMessages()
#endif
// Forward the request messages
- for (rIter = this->requestMsgs.begin(); rIter != this->requestMsgs.end();
- ++rIter)
+ for (auto rIter = this->requestMsgs.begin();
+ rIter != this->requestMsgs.end(); ++rIter)
{
msg = this->PackOutgoingTopicMsg(this->requestTopic,
pb2json(*(*rIter).get()));
@@ -789,7 +903,7 @@ void GazeboInterface::ProcessMessages()
this->requestMsgs.clear();
// Forward the stats messages.
- for (wIter = this->statsMsgs.begin(); wIter != this->statsMsgs.end();
+ for (auto wIter = this->statsMsgs.begin(); wIter != this->statsMsgs.end();
++wIter)
{
msg = this->PackOutgoingTopicMsg(this->statsTopic,
@@ -799,7 +913,7 @@ void GazeboInterface::ProcessMessages()
this->statsMsgs.clear();
// Forward all the pose messages.
- pIter = this->poseMsgs.begin();
+ auto pIter = this->poseMsgs.begin();
while (pIter != this->poseMsgs.end())
{
msg = this->PackOutgoingTopicMsg(this->poseTopic,
@@ -808,7 +922,6 @@ void GazeboInterface::ProcessMessages()
++pIter;
}
this->poseMsgs.clear();
-
this->receivedMessage = false;
}
}
@@ -818,7 +931,7 @@ void GazeboInterface::ProcessServiceRequests()
{
std::vector<std::string> services;
{
- boost::recursive_mutex::scoped_lock lock(*this->serviceMutex);
+ std::lock_guard<std::recursive_mutex> lock(this->serviceMutex);
services = this->serviceRequests;
this->serviceRequests.clear();
}
@@ -863,11 +976,11 @@ void GazeboInterface::OnModelMsg(ConstModelPtr &_msg)
if (!this->IsConnected())
return;
- boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
+ std::unique_lock<std::recursive_mutex> lock(this->receiveMutex);
this->modelMsgs.push_back(_msg);
this->receivedMessage = true;
lock.unlock();
- this->receiveCondition->notify_one();
+ this->receiveCondition.notify_one();
}
/////////////////////////////////////////////////
@@ -879,7 +992,6 @@ bool GazeboInterface::FilterPoses(const TimedPose &_old,
// double ratio = 100.0 * this->skippedMsgCount / this->messageWindowSize;
// std::cout << "Message filter: " << ratio << " %" << std::endl;
// std::cout << "Message count : " << this->skippedMsgCount;
- // std::cout << " " << << std::endl;
this->skippedMsgCount = 0;
this->messageCount = 0;
}
@@ -941,7 +1053,7 @@ void GazeboInterface::OnPoseMsg(ConstPosesStampedPtr &_msg)
if (!this->IsConnected())
return;
- boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
+ std::unique_lock<std::recursive_mutex> lock(this->receiveMutex);
PoseMsgs_L::iterator iter;
for (int i = 0; i < _msg->pose_size(); ++i)
@@ -976,7 +1088,7 @@ void GazeboInterface::OnPoseMsg(ConstPosesStampedPtr &_msg)
else
{
TimedPose oldPose = it->second;
- filtered = FilterPoses(oldPose, currentPose);
+ filtered = this->FilterPoses(oldPose, currentPose);
if (!filtered)
{
// update the map
@@ -989,7 +1101,7 @@ void GazeboInterface::OnPoseMsg(ConstPosesStampedPtr &_msg)
std::cout.flush();
this->receivedMessage = true;
lock.unlock();
- this->receiveCondition->notify_one();
+ this->receiveCondition.notify_one();
}
/////////////////////////////////////////////////
@@ -998,11 +1110,11 @@ void GazeboInterface::OnRequest(ConstRequestPtr &_msg)
if (!this->IsConnected())
return;
- boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
+ std::unique_lock<std::recursive_mutex> lock(this->receiveMutex);
this->requestMsgs.push_back(_msg);
this->receivedMessage = true;
lock.unlock();
- this->receiveCondition->notify_one();
+ this->receiveCondition.notify_one();
}
/////////////////////////////////////////////////
@@ -1018,7 +1130,8 @@ void GazeboInterface::OnResponse(ConstResponsePtr &_msg)
{
gazebo::msgs::Scene sceneMsg;
sceneMsg.ParseFromString(_msg->serialized_data());
- boost::shared_ptr<gazebo::msgs::Scene> sm(new gazebo::msgs::Scene(sceneMsg));
+ boost::shared_ptr<gazebo::msgs::Scene> sm(
+ new gazebo::msgs::Scene(sceneMsg));
this->sceneMsgs.push_back(sm);
this->requests.erase(_msg->id());
}
@@ -1026,12 +1139,13 @@ void GazeboInterface::OnResponse(ConstResponsePtr &_msg)
{
gazebo::msgs::Physics physicsMsg;
physicsMsg.ParseFromString(_msg->serialized_data());
- boost::shared_ptr<gazebo::msgs::Physics> pm(new gazebo::msgs::Physics(physicsMsg));
+ boost::shared_ptr<gazebo::msgs::Physics> pm(
+ new gazebo::msgs::Physics(physicsMsg));
this->physicsMsgs.push_back(pm);
this->requests.erase(_msg->id());
}
this->receivedMessage = true;
- this->receiveCondition->notify_one();
+ this->receiveCondition.notify_one();
}
/////////////////////////////////////////////////
@@ -1040,11 +1154,11 @@ void GazeboInterface::OnLightFactoryMsg(ConstLightPtr &_msg)
if (!this->IsConnected())
return;
- boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
+ std::unique_lock<std::recursive_mutex> lock(this->receiveMutex);
this->lightFactoryMsgs.push_back(_msg);
this->receivedMessage = true;
lock.unlock();
- this->receiveCondition->notify_one();
+ this->receiveCondition.notify_one();
}
/////////////////////////////////////////////////
@@ -1053,11 +1167,11 @@ void GazeboInterface::OnLightModifyMsg(ConstLightPtr &_msg)
if (!this->IsConnected())
return;
- boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
+ std::unique_lock<std::recursive_mutex> lock(this->receiveMutex);
this->lightModifyMsgs.push_back(_msg);
this->receivedMessage = true;
lock.unlock();
- this->receiveCondition->notify_one();
+ this->receiveCondition.notify_one();
}
/////////////////////////////////////////////////
@@ -1066,11 +1180,11 @@ void GazeboInterface::OnScene(ConstScenePtr &_msg)
if (!this->IsConnected())
return;
- boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
+ std::unique_lock<std::recursive_mutex> lock(this->receiveMutex);
this->sceneMsgs.push_back(_msg);
this->receivedMessage = true;
lock.unlock();
- this->receiveCondition->notify_one();
+ this->receiveCondition.notify_one();
}
/////////////////////////////////////////////////
@@ -1079,49 +1193,56 @@ void GazeboInterface::OnPhysicsMsg(ConstPhysicsPtr &_msg)
if (!this->IsConnected())
return;
- boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
+ std::unique_lock<std::recursive_mutex> lock(this->receiveMutex);
this->physicsMsgs.push_back(_msg);
this->receivedMessage = true;
lock.unlock();
- this->receiveCondition->notify_one();
+ this->receiveCondition.notify_one();
}
/////////////////////////////////////////////////
void GazeboInterface::OnStats(ConstWorldStatisticsPtr &_msg)
{
+ // store stats msg. This is sent to all clients when they first connect to
+ // the bridge to determine if gazebo is in sim or playback mode
+ this->statsMsg = _msg;
+
if (!this->IsConnected())
return;
gazebo::common::Time wallTime;
wallTime = gazebo::msgs::Convert(_msg->real_time());
+
+ gazebo::common::Time lastStatsTime;
+ if (this->lastStatsMsg)
+ lastStatsTime = gazebo::msgs::Convert(this->lastStatsMsg->real_time());
+ // bool playback = this->lastStatsMsg->has_log_playback_stats();
+ double timeDelta = (wallTime - lastStatsTime).Double();
bool paused = _msg->paused();
// pub at 1Hz, but force pub if world state changes
- if (((wallTime - this->lastStatsTime).Double() >= 1.0) ||
- wallTime < this->lastStatsTime ||
+ if (timeDelta >= 1.0 || wallTime < lastStatsTime ||
this->lastPausedState != paused)
{
- this->lastStatsTime = wallTime;
this->lastPausedState = paused;
- boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
+ this->lastStatsMsg = _msg;
+
+ std::unique_lock<std::recursive_mutex> lock(this->receiveMutex);
this->statsMsgs.push_back(_msg);
this->receivedMessage = true;
lock.unlock();
- this->receiveCondition->notify_one();
+ this->receiveCondition.notify_one();
}
}
/////////////////////////////////////////////////
void GazeboInterface::OnRoad(ConstRoadPtr &_msg)
{
- if (!this->IsConnected())
- return;
-
- boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
+ std::unique_lock<std::recursive_mutex> lock(this->receiveMutex);
this->roadMsgs.push_back(_msg);
this->receivedMessage = true;
lock.unlock();
- this->receiveCondition->notify_one();
+ this->receiveCondition.notify_one();
}
/////////////////////////////////////////////////
@@ -1130,11 +1251,11 @@ void GazeboInterface::OnJointMsg(ConstJointPtr &_msg)
if (!this->IsConnected())
return;
- boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
+ std::unique_lock<std::recursive_mutex> lock(this->receiveMutex);
this->jointMsgs.push_back(_msg);
this->receivedMessage = true;
lock.unlock();
- this->receiveCondition->notify_one();
+ this->receiveCondition.notify_one();
}
#ifdef GAZEBO_HBP_SUPPORT_JOINT_STATE_MESSAGES
@@ -1144,11 +1265,11 @@ void GazeboInterface::OnJointStateMsg(ConstJointStatePtr &_msg)
if (!this->IsConnected())
return;
- boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
+ std::unique_lock<std::recursive_mutex> lock(this->receiveMutex);
this->jointStateMsgs.push_back(_msg);
this->receivedMessage = true;
lock.unlock();
- this->receiveCondition->notify_one();
+ this->receiveCondition.notify_one();
}
#endif
@@ -1158,11 +1279,11 @@ void GazeboInterface::OnMuscleVisualizationMsg(ConstOpenSimMusclesPtr &_msg)
if (!this->IsConnected())
return;
- boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
+ std::unique_lock<std::recursive_mutex> lock(this->receiveMutex);
this->muscleVisualizationMsgs.push_back(_msg);
this->receivedMessage = true;
lock.unlock();
- this->receiveCondition->notify_one();
+ this->receiveCondition.notify_one();
}
#endif
@@ -1172,11 +1293,11 @@ void GazeboInterface::OnSensorMsg(ConstSensorPtr &_msg)
if (!this->IsConnected())
return;
- boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
+ std::unique_lock<std::recursive_mutex> lock(this->receiveMutex);
this->sensorMsgs.push_back(_msg);
this->receivedMessage = true;
lock.unlock();
- this->receiveCondition->notify_one();
+ this->receiveCondition.notify_one();
}
/////////////////////////////////////////////////
@@ -1185,11 +1306,11 @@ void GazeboInterface::OnVisualMsg(ConstVisualPtr &_msg)
if (!this->IsConnected())
return;
- boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
+ std::unique_lock<std::recursive_mutex> lock(this->receiveMutex);
this->visualMsgs.push_back(_msg);
this->receivedMessage = true;
lock.unlock();
- this->receiveCondition->notify_one();
+ this->receiveCondition.notify_one();
}
/////////////////////////////////////////////////
@@ -1219,63 +1340,52 @@ std::string GazeboInterface::PackOutgoingServiceMsg(const std::string &_id,
/////////////////////////////////////////////////
void GazeboInterface::Send(const std::string &_msg)
{
-// if (this->socketServer)
-// this->socketServer->Write(_msg);
- boost::recursive_mutex::scoped_lock lock(outgoingMutex);
+ std::lock_guard<std::recursive_mutex> lock(this->outgoingMutex);
if (outgoing.size() < MAX_NUM_MSG_SIZE)
- outgoing.push_back(_msg);
+ this->outgoing.push_back(_msg);
}
-/*
-/////////////////////////////////////////////////
-void GazeboInterface::Write(const std::string &_msg)
-{
- boost::recursive_mutex::scoped_lock lock(outgoingMutex);
- if (outgoing.size() < MAX_NUM_MSG_SIZE)
- outgoing.push_back(_msg);
-}*/
-
/////////////////////////////////////////////////
std::vector<std::string> GazeboInterface::PopIncomingMessages()
{
- boost::recursive_mutex::scoped_lock lock(incomingMutex);
- std::vector<std::string> in = incoming;
- incoming.clear();
+ std::lock_guard<std::recursive_mutex> lock(this->incomingMutex);
+ std::vector<std::string> in = this->incoming;
+ this->incoming.clear();
return in;
}
/////////////////////////////////////////////////
void GazeboInterface::ClearIncomingMessages()
{
- boost::recursive_mutex::scoped_lock lock(incomingMutex);
- incoming.clear();
+ std::lock_guard<std::recursive_mutex> lock(incomingMutex);
+ this->incoming.clear();
}
/////////////////////////////////////////////////
std::vector<std::string> GazeboInterface::PopOutgoingMessages()
{
- boost::recursive_mutex::scoped_lock lock(outgoingMutex);
- std::vector<std::string> out = outgoing;
- outgoing.clear();
+ std::lock_guard<std::recursive_mutex> lock(this->outgoingMutex);
+ std::vector<std::string> out = this->outgoing;
+ this->outgoing.clear();
return out;
}
/////////////////////////////////////////////////
void GazeboInterface::ClearOutgoingMessages()
{
- boost::recursive_mutex::scoped_lock lock(outgoingMutex);
+ std::lock_guard<std::recursive_mutex> lock(outgoingMutex);
outgoing.clear();
}
/////////////////////////////////////////////////
void GazeboInterface::PushRequest(const std::string &_msg)
{
- boost::recursive_mutex::scoped_lock lock(incomingMutex);
+ std::unique_lock<std::recursive_mutex> lock(this->incomingMutex);
if (incoming.size() < MAX_NUM_MSG_SIZE)
- incoming.push_back(_msg);
+ this->incoming.push_back(_msg);
this->receivedMessage = true;
lock.unlock();
- this->receiveCondition->notify_one();
+ this->receiveCondition.notify_one();
}
/////////////////////////////////////////////////
@@ -1286,46 +1396,84 @@ void GazeboInterface::LoadMaterialScripts(const std::string &_path)
}
/////////////////////////////////////////////////
-void GazeboInterface::WaitForServiceRequest() const
+void GazeboInterface::WaitForConnection()
{
- boost::recursive_mutex::scoped_lock lock(*this->serviceMutex);
- while (this->serviceRequests.empty())
+ std::unique_lock<std::mutex> lock(this->connectionMutex);
+ while (!this->isConnected)
{
- this->serviceCondition->wait(lock);
+ this->connectionCondition.wait(lock);
}
}
/////////////////////////////////////////////////
-void GazeboInterface::WaitForMessage() const
+void GazeboInterface::SetConnected(bool _connected)
{
- boost::recursive_mutex::scoped_lock lock(*this->receiveMutex);
- while (!this->receivedMessage)
- {
- this->receiveCondition->wait(lock);
- }
+ std::lock_guard<std::mutex> lock(this->connectionMutex);
+ this->isConnected = _connected;
+ this->connectionCondition.notify_all();
}
/////////////////////////////////////////////////
-void GazeboInterface::WaitForConnection() const
+bool GazeboInterface::IsConnected()
{
- boost::mutex::scoped_lock lock(*this->connectionMutex);
- while (!this->isConnected)
- {
- this->connectionCondition->wait(lock);
- }
+ std::lock_guard<std::mutex> lock(this->connectionMutex);
+ return this->isConnected;
}
/////////////////////////////////////////////////
-void GazeboInterface::SetConnected(bool _connected)
+void GazeboInterface::SetPoseFilterMinimumDistanceSquared(double _m)
{
- boost::mutex::scoped_lock lock(*this->connectionMutex);
- this->isConnected = _connected;
- this->connectionCondition->notify_all();
+ this->minimumDistanceSquared = _m;
}
/////////////////////////////////////////////////
-bool GazeboInterface::IsConnected() const
+double GazeboInterface::GetPoseFilterMinimumDistanceSquared()
{
- boost::mutex::scoped_lock lock(*this->connectionMutex);
- return this->isConnected;
+ return this->minimumDistanceSquared;
+}
+
+/////////////////////////////////////////////////
+void GazeboInterface::SetPoseFilterMinimumQuaternionSquared(double _m)
+{
+ this->minimumQuaternionSquared = _m;
+}
+
+/////////////////////////////////////////////////
+double GazeboInterface::GetPoseFilterMinimumQuaternionSquared()
+{
+ return this->minimumQuaternionSquared;
+}
+
+/////////////////////////////////////////////////
+void GazeboInterface::SetPoseFilterMinimumMsgAge(double _m)
+{
+ this->minimumMsgAge = _m;
+}
+
+/////////////////////////////////////////////////
+double GazeboInterface::GetPoseFilterMinimumMsgAge()
+{
+ return this->minimumMsgAge;
}
+
+/////////////////////////////////////////////////
+void GazeboInterface::WaitForMessage()
+{
+ std::unique_lock<std::recursive_mutex> lock(this->receiveMutex);
+ while (!this->receivedMessage)
+ {
+ this->receiveCondition.wait(lock);
+ }
+}
+
+/////////////////////////////////////////////////
+void GazeboInterface::WaitForServiceRequest()
+{
+ std::unique_lock<std::recursive_mutex> lock(this->serviceMutex);
+ while (this->serviceRequests.empty())
+ {
+ this->serviceCondition.wait(lock);
+ }
+}
+
+
diff --git a/gzbridge/GazeboInterface.hh b/gzbridge/GazeboInterface.hh
index 7960ee8..5e1ec7d 100644
--- a/gzbridge/GazeboInterface.hh
+++ b/gzbridge/GazeboInterface.hh
@@ -15,12 +15,16 @@
*
*/
-#ifndef _GAZEBO_INTERFACE_HH_
-#define _GAZEBO_INTERFACE_HH_
+#ifndef GZBRIDGE_GAZEBOINTERFACE_HH_
+#define GZBRIDGE_GAZEBOINTERFACE_HH_
#include <string>
#include <list>
#include <map>
+#include <memory>
+#include <mutex>
+#include <thread>
+#include <condition_variable>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/transport/TransportIface.hh>
@@ -33,12 +37,6 @@
#pragma message("Found no HBP Gazebo version : disabling HBP features")
#endif
-
-namespace boost
-{
- class thread;
-}
-
namespace gzweb
{
class OgreMaterialParser;
@@ -93,7 +91,7 @@ namespace gzweb
/// \brief Get the connected state
/// \return True if there are client connections.
- public: bool IsConnected() const;
+ public: bool IsConnected();
/// \brief Pack topic publish message.
private: std::string PackOutgoingTopicMsg(const std::string &_topic,
@@ -185,13 +183,13 @@ namespace gzweb
private: void OnResponse(ConstResponsePtr &_msg);
/// \brief Block if there are no connections.
- private: void WaitForConnection() const;
+ private: void WaitForConnection();
/// \brief Block if there are no service requests.
- private: void WaitForServiceRequest() const;
+ private: void WaitForServiceRequest();
/// \brief Block if there are no messages.
- private: void WaitForMessage() const;
+ private: void WaitForMessage();
/// \brief a pose at a specific time
typedef std::pair<gazebo::common::Time, ignition::math::Pose3d> TimedPose;
@@ -204,16 +202,16 @@ namespace gzweb
const TimedPose &_current);
/// \brief Incoming messages.
- public: static std::vector<std::string> incoming;
+ public: std::vector<std::string> incoming;
/// \brief Outgoing messages.
- public: static std::vector<std::string> outgoing;
+ public: std::vector<std::string> outgoing;
/// \brief Thread to run the main loop.
- private: boost::thread *runThread;
+ private: std::unique_ptr<std::thread> runThread;
/// \brief Thread for processing services requests.
- private: boost::thread *serviceThread;
+ private: std::unique_ptr<std::thread> serviceThread;
/// \brief Gazebo transport node.
private: gazebo::transport::NodePtr node;
@@ -285,29 +283,38 @@ namespace gzweb
/// \brief Subscribe to reponses.
private: gazebo::transport::SubscriberPtr responseSub;
+ /// \brief Publish playback control messages
+ private: gazebo::transport::PublisherPtr playbackControlPub;
+
/// \brief Request message for getting initial scene info.
private: std::map<int, gazebo::msgs::Request *> requests;
/// \brief Mutex to lock the various message buffers.
- private: boost::recursive_mutex *receiveMutex;
+ private: std::recursive_mutex receiveMutex;
/// \brief Mutex to lock the service request buffer.
- private: boost::recursive_mutex *serviceMutex;
+ private: std::recursive_mutex serviceMutex;
+
+ /// \brief Mutex to lock the incoming message request buffer.
+ private: std::recursive_mutex incomingMutex;
+
+ /// \brief Mutex to lock the outgoing message request buffer.
+ private: std::recursive_mutex outgoingMutex;
/// \brief Mutex to protect the isConnected variable.
- private: boost::mutex *connectionMutex;
+ private: std::mutex connectionMutex;
/// \brief The condition to notify when connection state changes.
- private: boost::condition_variable *connectionCondition;
+ public: std::condition_variable connectionCondition;
/// \brief The condition to notify the service thread when new requests arrived.
- private: boost::condition_variable_any *serviceCondition;
+ private: std::condition_variable_any serviceCondition;
/// \brief The number of received messages not processed so far
private: bool receivedMessage;
/// \brief The condition to notify the message thread when new messages arrived.
- private: boost::condition_variable_any *receiveCondition;
+ private: std::condition_variable_any receiveCondition;
/// \def ModelMsgs_L
/// \brief List of model messages.
@@ -428,7 +435,7 @@ namespace gzweb
private: std::vector<std::string> serviceRequests;
/// \brief True to stop the interface.
- private: bool stop;
+ private: bool stop = false;
/// \brief Name of sensor topic.
private: std::string sensorTopic;
@@ -498,17 +505,25 @@ namespace gzweb
/// \brief Name of delete topic.
private: std::string deleteTopic;
+ /// \brief Name of playback control topic.
+ private: std::string playbackControlTopic;
+
/// \brief Ogre material parser.
- private: OgreMaterialParser *materialParser;
+ private: OgreMaterialParser *materialParser = nullptr;
- /// \brief Last world stats time received
- private: gazebo::common::Time lastStatsTime;
+ /// \brief Last world stats msg received
+ private: boost::shared_ptr<gazebo::msgs::WorldStatistics const>
+ lastStatsMsg;
+
+ /// \brief Most recent world stats msg received
+ private: boost::shared_ptr<gazebo::msgs::WorldStatistics const>
+ statsMsg;
/// \brief Last world state received, play or paused.
private: bool lastPausedState;
/// \brief filter pose message based on minimum distance criteria
- private: double minimumDistanceSquared;
+ private: double minimumDistanceSquared = 0;
/// \brief filter pose message based on minimum rotation criteria
private: double minimumQuaternionSquared;
@@ -519,45 +534,22 @@ namespace gzweb
/// \brief filter pose message based on minimum rotation criteria when message arrives too early
private: double minimumQuaternionSquaredTooEarly;
-
/// \brief filter pose message based on minimum elapsed time (seconds)
- private: double minimumMsgAge;
+ private: double minimumMsgAge = 0;
- private: int skippedMsgCount;
- private: int messageWindowSize;
- private: int messageCount;
+ private: int skippedMsgCount = 0;
+ private: int messageWindowSize = 0;
+ private: int messageCount = 0;
/// \brief True if there is a client connection.
- private: bool isConnected;
-
- public: inline void SetPoseFilterMinimumDistanceSquared(double m)
- {
- this->minimumDistanceSquared = m;
- }
- public: inline double GetPoseFilterMinimumDistanceSquared()
- {
- return this->minimumDistanceSquared;
- }
-
- public: inline void SetPoseFilterMinimumQuaternionSquared(double m)
- {
- this->minimumQuaternionSquared = m;
- }
-
- public: inline double GetPoseFilterMinimumQuaternionSquared()
- {
- return this->minimumQuaternionSquared;
- }
-
- public: inline void SetPoseFilterMinimumMsgAge(double m)
- {
- this->minimumMsgAge = m;
- }
-
- public: inline double GetPoseFilterMinimumMsgAge()
- {
- return this->minimumMsgAge;
- }
+ private: bool isConnected = false;
+
+ public: void SetPoseFilterMinimumDistanceSquared(double _m);
+ public: double GetPoseFilterMinimumDistanceSquared();
+ public: void SetPoseFilterMinimumQuaternionSquared(double _m);
+ public: double GetPoseFilterMinimumQuaternionSquared();
+ public: void SetPoseFilterMinimumMsgAge(double _m);
+ public: double GetPoseFilterMinimumMsgAge();
};
}
diff --git a/gzbridge/OgreMaterialParser.cc b/gzbridge/OgreMaterialParser.cc
index 12b2cb3..acc1657 100644
--- a/gzbridge/OgreMaterialParser.cc
+++ b/gzbridge/OgreMaterialParser.cc
@@ -62,7 +62,6 @@ std::string OgreMaterialParser::GetMaterialAsJson() const
std::map<std::string, ConfigNode *>::iterator it;
bool first = true;
-
for (it = scripts.begin(); it != scripts.end(); ++it)
{
std::string name = it->first;
@@ -122,6 +121,12 @@ std::string OgreMaterialParser::GetMaterialAsJson() const
if (textureNode)
{
std::string textureStr = textureNode->getValue(0);
+ index = textureStr.rfind(".");
+ if (index != std::string::npos)
+ {
+ textureStr = textureStr.substr(0, index+1) + "png";
+ }
+
jsonStr += "\"texture\":\"" + textureStr + "\",";
}
ConfigNode *scaleNode = textureUnitNode->findChild("scale");
diff --git a/gzbridge/OgreMaterialParser.hh b/gzbridge/OgreMaterialParser.hh
index d5797d0..afdb887 100644
--- a/gzbridge/OgreMaterialParser.hh
+++ b/gzbridge/OgreMaterialParser.hh
@@ -1,5 +1,5 @@
-#ifndef _OGRE_MATERIAL_PARSER_HH_
-#define _OGRE_MATERIAL_PARSER_HH_
+#ifndef GZBRIDGE_OGREMATERIALPARSER_HH_
+#define GZBRIDGE_OGREMATERIALPARSER_HH_
#include <string>
@@ -18,7 +18,7 @@ namespace gzweb
public: std::string GetMaterialAsJson() const;
- private: ConfigLoader *configLoader;
+ private: ConfigLoader *configLoader = nullptr;
};
}
diff --git a/gzbridge/pb2json.cc b/gzbridge/pb2json.cc
index dc39e6d..c85a9d3 100644
--- a/gzbridge/pb2json.cc
+++ b/gzbridge/pb2json.cc
@@ -43,22 +43,26 @@ namespace gzweb {
return output;
}
- char *pb2json(const Message &msg)
+ std::string pb2json(const Message &msg)
{
json_t *root = parse_msg(&msg);
char *json = json_dumps(root, 0);
+ std::string str(json);
json_decref(root);
- return json; // should be freed by caller
+ delete json;
+ return str;
}
- char *pb2json(Message *msg, const char *buf, int len)
+ std::string pb2json(Message *msg, const char *buf, int len)
{
std::string s (buf, len);
msg->ParseFromString(s);
json_t *root = parse_msg(msg);
char *json = json_dumps(root, 0);
+ std::string str(json);
json_decref(root);
- return json; // should be freed by caller
+ delete json;
+ return str;
}
json_t *parse_repeated_field(const Message *msg,
@@ -294,13 +298,126 @@ namespace gzweb {
return "";
}
- std::string get_value(const std::string &msg, const std::string &key)
+ std::string get_value(const std::string &_msg, const std::string &_key)
{
json_t *root;
json_error_t error;
- root = json_loads(msg.c_str(), 0, &error);
- std::string result = get(root, key);
+ root = json_loads(_msg.c_str(), 0, &error);
+ std::string result = get(root, _key);
json_decref(root);
return result;
}
+
+ /// \brief Private class of the JsonObj
+ class JsonObjPrivate
+ {
+ /// \brief Pointer to internal json object
+ public: json_t *json = nullptr;
+
+ /// \brief True if we should decrement ref count on internal json obj
+ public: bool decref = true;
+ };
+}
+
+using namespace gzweb;
+
+//////////////////////////////////////////////////
+JsonObj::JsonObj(const std::string &_str)
+ : dataPtr(new JsonObjPrivate)
+{
+ json_error_t error;
+ this->dataPtr->json = json_loads(_str.c_str(), 0, &error);
+}
+
+//////////////////////////////////////////////////
+JsonObj::JsonObj(json_t *_obj)
+ : dataPtr(new JsonObjPrivate)
+{
+ this->dataPtr->json = _obj;
+ this->dataPtr->decref = false;
+}
+
+//////////////////////////////////////////////////
+JsonObj::JsonObj(const JsonObj &_other)
+ : dataPtr(new JsonObjPrivate)
+{
+ *this->dataPtr = *_other.dataPtr;
+}
+
+//////////////////////////////////////////////////
+JsonObj::~JsonObj()
+{
+ if (this->dataPtr->decref)
+ json_decref(this->dataPtr->json);
+}
+
+//////////////////////////////////////////////////
+JsonObj JsonObj::Object(const std::string &_key) const
+{
+ if (json_is_object(this->dataPtr->json))
+ {
+ json_t *obj = json_object_get(this->dataPtr->json, _key.c_str());
+ return JsonObj(obj);
+ }
+ return JsonObj(nullptr);
+}
+
+//////////////////////////////////////////////////
+double JsonObj::Number() const
+{
+ if (json_is_number(this->dataPtr->json))
+ {
+ return json_number_value(this->dataPtr->json);
+ }
+ return 0.0;
+}
+
+//////////////////////////////////////////////////
+bool JsonObj::Bool() const
+{
+ if (json_is_boolean(this->dataPtr->json))
+ {
+ return json_boolean_value(this->dataPtr->json);
+ }
+ return false;
+}
+
+//////////////////////////////////////////////////
+std::string JsonObj::String() const
+{
+ if (json_is_string(this->dataPtr->json))
+ {
+ return json_string_value(this->dataPtr->json);
+ }
+ return std::string();
+}
+
+
+//////////////////////////////////////////////////
+JsonObj JsonObj::ArrayObject(const unsigned int _index) const
+{
+ if (json_is_array(this->dataPtr->json))
+ {
+ json_t *obj = json_array_get(this->dataPtr->json, _index);
+ return JsonObj(obj);
+ }
+
+ return JsonObj(nullptr);
+}
+
+//////////////////////////////////////////////////
+unsigned int JsonObj::ArraySize() const
+{
+ if (json_is_array(this->dataPtr->json))
+ {
+ return json_array_size(this->dataPtr->json);
+ }
+
+ return 0u;
+}
+
+//////////////////////////////////////////////////
+JsonObj::operator bool() const
+{
+ return this->dataPtr->json != nullptr;
}
diff --git a/gzbridge/pb2json.hh b/gzbridge/pb2json.hh
index 3ece65e..a5bfd10 100644
--- a/gzbridge/pb2json.hh
+++ b/gzbridge/pb2json.hh
@@ -15,8 +15,10 @@
*
*/
-#ifndef _PB2JSON_H_
-#define _PB2JSON_H_
+#ifndef GZBRIDGE_PB2JSON_HH_
+#define GZBRIDGE_PB2JSON_HH_
+
+#include <memory>
#include <google/protobuf/descriptor.h>
#include <google/protobuf/message.h>
@@ -24,9 +26,9 @@
namespace gzweb
{
- char *pb2json(const google::protobuf::Message &msg);
+ std::string pb2json(const google::protobuf::Message &msg);
- char *pb2json(google::protobuf::Message *msg,const char *buf,int len);
+ std::string pb2json(google::protobuf::Message *msg,const char *buf,int len);
std::string get_value(const std::string &msg, const std::string &key);
@@ -35,6 +37,63 @@ namespace gzweb
json_t *parse_repeated_field(const google::protobuf::Message *msg,
const google::protobuf::Reflection *ref,
const google::protobuf::FieldDescriptor *field);
+
+
+ // forward declaration
+ class JsonObjPrivate;
+
+ /// \brief C++ interface for manipulating json object
+ class JsonObj
+ {
+ /// \brief Constructor
+ /// \param[in] _str input json string
+ public: JsonObj(const std::string &_str);
+
+ /// \brief Copy constructor
+ /// \param[in] _other Other JsonObj
+ public: JsonObj(const JsonObj &_other);
+
+ /// \brief Destructor
+ public: ~JsonObj();
+
+ /// \brief Get child json object
+ /// \param[in] _key Json key
+ /// \return JsonObj, false if key not found.
+ public: JsonObj Object(const std::string &_key) const;
+
+ /// \brief Get number value
+ /// \return Double value, zero if value is not a number.
+ public: double Number() const;
+
+ /// \brief Get bool value
+ /// \return Bool value, always false if value is not a bool.
+ public: bool Bool() const;
+
+ /// \brief Get string value
+ /// \return String value, empty if value is not a string.
+ public: std::string String() const;
+
+ /// \brief If this object is an array, get json object at specified
+ /// array index.
+ /// \param[in] _index Index in the array
+ /// \return JsonObj array, false if not an array.
+ public: JsonObj ArrayObject(const unsigned int _index) const;
+
+ /// \brief If this object is an array, get the size of the array
+ /// \return Size of array, 0u if not an array.
+ public: unsigned int ArraySize() const;
+
+ /// \brief Bool operator
+ /// \return True if pointer to json object is not null
+ public: operator bool() const;
+
+ /// \brief Private constructor
+ /// \param[in] _obj Internal json object
+ private: JsonObj(json_t *_obj);
+
+ /// \brief Internal private data pointer.
+ private: std::unique_ptr<JsonObjPrivate> dataPtr;
+ };
}
#endif
diff --git a/gzbridge/server.js b/gzbridge/server.js
new file mode 100755
index 0000000..1a16eef
--- /dev/null
+++ b/gzbridge/server.js
@@ -0,0 +1,175 @@
+#!/usr/bin/env node
+
+"use strict"
+
+const debug = require('debug')('ws_server');
+
+if (process.argv.length != 2 && process.argv.length != 4) {
+ console.error('Server called with wrong parameters: usage ws_server [gzserver_host gzserver_port]')
+ process.exit(1);
+}
+
+const WebSocketServer = require('websocket').server;
+const http = require('http');
+const gzbridge = require('./build/Release/gzbridge');
+
+/**
+ * Path from where the static site is served
+ */
+const staticBasePath = './../http/client';
+
+/**
+ * Port to connect websocket
+ */
+const wsPort = 7681;
+
+/**
+ * Array of websocket connections currently active
+ */
+let connections = [];
+
+/**
+ * Holds the message containing all material scripts in case there is no
+ * gzserver connected
+ */
+let materialScriptsMessage = {};
+
+/**
+ * Whether the websocket is connected to a gzserver
+ */
+let isConnected = false;
+
+// process.argv[2] is gzserver host, process.argv[3] is gzserver port.
+let gzNode =
+ (process.argv.length === 2) ?
+ new gzbridge.GZNode() : new gzbridge.GZNode(process.argv[2], process.argv[3]);
+
+if (gzNode.getIsGzServerConnected()) {
+ gzNode.loadMaterialScripts(staticBasePath + '/assets');
+
+ var x = parseFloat(process.env.GZBRIDGE_POSE_FILTER_DELTA_TRANSLATION || '0.001')
+ gzNode.setPoseMsgFilterMinimumDistanceSquared(x);
+ var x = parseFloat(process.env.GZBRIDGE_POSE_FILTER_DELTA_ROTATION || '0.001')
+ gzNode.setPoseMsgFilterMinimumQuaternionSquared(x);
+ var x = parseFloat(process.env.GZBRIDGE_UPDATE_EARLY_THRESHOLD || '0.02')
+ gzNode.setPoseMsgFilterMinimumAge(x);
+
+ console.log('Pose message filter parameters: ');
+ console.log(' minimum seconds between successive messages: ' +
+ gzNode.getPoseMsgFilterMinimumAge());
+ console.log(' minimum XYZ distance squared between successive messages: ' +
+ gzNode.getPoseMsgFilterMinimumDistanceSquared());
+ console.log(' minimum Quaternion distance squared between successive messages:'
+ + ' ' + gzNode.getPoseMsgFilterMinimumQuaternionSquared());
+ console.log('--------------------------------------------------------------');
+} else {
+ materialScriptsMessage =
+ gzNode.getMaterialScriptsMessage(staticBasePath + '/assets');
+}
+
+let httpServer = http.createServer(function (request, response) {
+ console.log(new Date() + ' Received request for ' + request.url);
+ response.writeHead(404);
+ response.end();
+});
+
+httpServer.listen(wsPort, function () {
+ console.log(new Date() + ' gzbridge Websocket is listening on port: ' + wsPort);
+});
+
+// Start websocket server
+let wsServer = new WebSocketServer({
+ httpServer: httpServer,
+ // You should not use autoAcceptConnections for production
+ // applications, as it defeats all standard cross-origin protection
+ // facilities built into the protocol and the browser. You should
+ // *always* verify the connection's origin and decide whether or not
+ // to accept it.
+ autoAcceptConnections: false
+});
+
+function originIsAllowed(_origin) {
+ // put logic here to detect whether the specified origin is allowed.
+ return true;
+}
+
+wsServer.on('request', function (request) {
+ console.log(new Date() + ' New request from: ' + request.origin);
+
+ if (!originIsAllowed(request.origin)) {
+ // Make sure we only accept requests from an allowed origin
+ request.reject();
+ console.log((new Date()) + ' Connection from origin ' +
+ request.origin + ' rejected.');
+ return;
+ }
+ // Accept request
+ let connection = request.accept(null, request.origin);
+
+ // If gzserver is not connected just send material scripts and status
+ if (!gzNode.getIsGzServerConnected()) {
+ // create error status and send it
+ let statusMessage =
+ '{"op":"publish","topic":"~/status","msg":{"status":"error"}}';
+ connection.sendUTF(statusMessage);
+ // send material scripts message
+ connection.sendUTF(materialScriptsMessage);
+ return;
+ }
+
+ connections.push(connection);
+
+ if (!isConnected) {
+ isConnected = true;
+ gzNode.setConnected(isConnected);
+ }
+
+ console.log(new Date() + ' New connection accepted from: ' + request.origin +
+ ' ' + connection.remoteAddress);
+
+ // Handle messages received from client
+ connection.on('message', function (message) {
+ debug("On message");
+
+ if (message.type === 'utf8') {
+ debug(new Date() + ' Received Message: ' + message.utf8Data +
+ ' from ' + request.origin + ' ' + connection.remoteAddress);
+ gzNode.request(message.utf8Data);
+ } else if (message.type === 'binary') {
+ debug(new Date() + ' Received Binary Message of ' +
+ message.binaryData.length + ' bytes from ' + request.origin + ' ' +
+ connection.remoteAddress);
+ connection.sendBytes(message.binaryData);
+ }
+ });
+ // Handle client disconnection
+ connection.on('close', function (reasonCode, description) {
+ console.log(new Date() + ' Peer ' + request.origin + ' ' +
+ connection.remoteAddress + ' disconnected.');
+
+ // remove connection from array
+ let conIndex = connections.indexOf(connection);
+ connections.splice(conIndex, 1);
+
+ // if there is no connection notify server that there is no connected client
+ if (connections.length === 0) {
+ isConnected = false;
+ gzNode.setConnected(isConnected);
+ }
+ });
+});
+
+// If not connected, periodically send messages
+if (gzNode.getIsGzServerConnected()) {
+ setInterval(function () {
+ if (connections.length > 0) {
+ let msgs = gzNode.getMessages();
+ for (let i = 0; i < connections.length; ++i) {
+ for (let j = 0; j < msgs.length; ++j) {
+ connections[i].sendUTF(msgs[j]);
+ }
+ }
+ }
+ }, 10);
+}
+
diff --git a/gzbridge/ws_server.js b/gzbridge/ws_server.js
deleted file mode 100755
index ef93f42..0000000
--- a/gzbridge/ws_server.js
+++ /dev/null
@@ -1,152 +0,0 @@
-#!/usr/bin/env node
-
-var debug = require('debug')('ws_server');
-
-if (process.argv.length != 2 && process.argv.length != 4) {
- console.error('Server called with wrong parameters: usage ws_server [gzserver_host gzserver_port]')
- process.exit(1);
-}
-
-var WebSocketServer = require('websocket').server;
-var http = require('http');
-
-var connections = [];
-var materialScriptsMessage = {};
-var addon = require('./build/Release/gzbridge');
-
-if (process.argv.length == 2) {
- var gzconnection = new addon.GZNode();
-}
-else {
- // process.argv[2] is gzserver host, process.argv[3] is gzserver port.
- var gzconnection = new addon.GZNode(process.argv[2], process.argv[3]);
-}
-
-if (gzconnection.getIsGzServerConnected())
-{
- gzconnection.loadMaterialScripts('../http/client/assets');
-
- var x = parseFloat(process.env.GZBRIDGE_POSE_FILTER_DELTA_TRANSLATION || '0.001')
- gzconnection.setPoseMsgFilterMinimumDistanceSquared(x);
- var x = parseFloat(process.env.GZBRIDGE_POSE_FILTER_DELTA_ROTATION || '0.001')
- gzconnection.setPoseMsgFilterMinimumQuaternionSquared(x);
- var x = parseFloat(process.env.GZBRIDGE_UPDATE_EARLY_THRESHOLD || '0.02')
- gzconnection.setPoseMsgFilterMinimumAge(x);
-
- console.log('Pose message filter parameters: ');
- console.log(' minimum seconds between successive messages: ' +
- gzconnection.getPoseMsgFilterMinimumAge());
- console.log(' minimum XYZ distance squared between successive messages: ' +
- gzconnection.getPoseMsgFilterMinimumDistanceSquared());
- console.log(' minimum Quaternion distance squared between successive messages:'
- + ' ' + gzconnection.getPoseMsgFilterMinimumQuaternionSquared());
-}
-else
-{
- materialScriptsMessage = gzconnection.getMaterialScriptsMessage('../http/client/assets');
-}
-
-var isConnected = false;
-
-var server = http.createServer(function(request, response) {
- console.log((new Date()) + ' Received request for ' + request.url);
- response.writeHead(404);
- response.end();
-});
-server.listen(7681, function() {
- console.log((new Date()) + ' Server is listening on port 7681');
-});
-
-wsServer = new WebSocketServer({
- httpServer: server,
- // You should not use autoAcceptConnections for production
- // applications, as it defeats all standard cross-origin protection
- // facilities built into the protocol and the browser. You should
- // *always* verify the connection's origin and decide whether or not
- // to accept it.
- autoAcceptConnections: false
-});
-
-function originIsAllowed(origin) {
- // put logic here to detect whether the specified origin is allowed.
- return true;
-}
-
-wsServer.on('request', function(request) {
- if (!originIsAllowed(request.origin)) {
- // Make sure we only accept requests from an allowed origin
- request.reject();
- console.log((new Date()) + ' Connection from origin ' +
- request.origin + ' rejected.');
- return;
- }
-
- var connection = request.accept(null, request.origin);
-
- // if gzserver is not connected just send material scripts and status
- if (!gzconnection.getIsGzServerConnected())
- {
- // create error status and send it
- var statusMessage = '{"op":"publish","topic":"~/status","msg":{"status":"error"}}';
- connection.sendUTF(statusMessage);
- // send material scripts message
- connection.sendUTF(materialScriptsMessage);
- return;
- }
-
- connections.push(connection);
-
- if (!isConnected)
- {
- isConnected = true;
- gzconnection.setConnected(isConnected);
- }
-
- console.log((new Date()) + ' Connection accepted.');
- connection.on('message', function(message) {
- if (message.type === 'utf8') {
- debug('Received Message: ' + message.utf8Data + ' from ' +
- request.origin + ' ' + connection.remoteAddress);
- gzconnection.request(message.utf8Data);
- }
- else if (message.type === 'binary') {
- debug('Received Binary Message of ' +
- message.binaryData.length + ' bytes');
- connection.sendBytes(message.binaryData);
- }
- });
- connection.on('close', function(reasonCode, description) {
- console.log((new Date()) + ' Peer ' + connection.remoteAddress +
- ' disconnected.');
-
- // remove connection from array
- var conIndex = connections.indexOf(connection);
- connections.splice(conIndex, 1);
-
- // if there is no connection notify server that there is no connected client
- if (connections.length == 0) {
- isConnected = false;
- gzconnection.setConnected(isConnected);
- }
- });
-});
-
-if (gzconnection.getIsGzServerConnected())
-{
- setInterval(update, 10);
-
- function update()
- {
- if (connections.length > 0)
- {
- var msgs = gzconnection.getMessages();
- for (var i = 0; i < connections.length; ++i)
- {
- for (var j = 0; j < msgs.length; ++j)
- {
- connections[i].sendUTF(msgs[j]);
- }
- }
- }
- }
-}
diff --git a/tools/gzcoarse.cc b/tools/gzcoarse.cc
index fbdad7d..589ed49 100644
--- a/tools/gzcoarse.cc
+++ b/tools/gzcoarse.cc
@@ -18,6 +18,7 @@
#include <gts.h>
#include <tinyxml.h>
#include <math.h>
+#include <gazebo/gazebo_config.h>
#include <gazebo/common/Mesh.hh>
#include <gazebo/common/Material.hh>
#include <gazebo/common/ColladaLoader.hh>
@@ -535,7 +536,11 @@ void ExportEffects(TiXmlElement *_libraryEffectsXml,
techniqueXml->LinkEndChild(phongXml);
// ambient
+#if GAZEBO_MAJOR_VERSION >= 9
+ unsigned int RGBAcolor = material->Ambient().AsRGBA();
+#else
unsigned int RGBAcolor = material->GetAmbient().GetAsRGBA();
+#endif
float r = ((RGBAcolor >> 24) & 0xFF) / 255.0f;
float g = ((RGBAcolor >> 16) & 0xFF) / 255.0f;
float b = ((RGBAcolor >> 8) & 0xFF) / 255.0f;
@@ -550,7 +555,11 @@ void ExportEffects(TiXmlElement *_libraryEffectsXml,
ambientXml->LinkEndChild(colorXml);
// emission
+#if GAZEBO_MAJOR_VERSION >= 9
+ RGBAcolor = material->Emissive().AsRGBA();
+#else
RGBAcolor = material->GetEmissive().GetAsRGBA();
+#endif
r = ((RGBAcolor >> 24) & 0xFF) / 255.0f;
g = ((RGBAcolor >> 16) & 0xFF) / 255.0f;
b = ((RGBAcolor >> 8) & 0xFF) / 255.0f;
@@ -578,7 +587,11 @@ void ExportEffects(TiXmlElement *_libraryEffectsXml,
}
else
{
+#if GAZEBO_MAJOR_VERSION >= 9
+ RGBAcolor = material->Diffuse().AsRGBA();
+#else
RGBAcolor = material->GetDiffuse().GetAsRGBA();
+#endif
r = ((RGBAcolor >> 24) & 0xFF) / 255.0f;
g = ((RGBAcolor >> 16) & 0xFF) / 255.0f;
b = ((RGBAcolor >> 8) & 0xFF) / 255.0f;
@@ -591,7 +604,11 @@ void ExportEffects(TiXmlElement *_libraryEffectsXml,
}
// specular
+#if GAZEBO_MAJOR_VERSION >= 9
+ RGBAcolor = material->Specular().AsRGBA();
+#else
RGBAcolor = material->GetSpecular().GetAsRGBA();
+#endif
r = ((RGBAcolor >> 24) & 0xFF) / 255.0f;
g = ((RGBAcolor >> 16) & 0xFF) / 255.0f;
b = ((RGBAcolor >> 8) & 0xFF) / 255.0f;
@@ -1287,10 +1304,17 @@ int main(int argc, char **argv)
gazebo::common::Material *outMaterial = new gazebo::common::Material();
outMaterial->SetTextureImage(inMaterial->GetTextureImage());
+#if GAZEBO_MAJOR_VERSION >= 9
+ outMaterial->SetAmbient(inMaterial->Ambient());
+ outMaterial->SetDiffuse(inMaterial->Diffuse());
+ outMaterial->SetSpecular(inMaterial->Specular());
+ outMaterial->SetEmissive(inMaterial->Emissive());
+#else
outMaterial->SetAmbient(inMaterial->GetAmbient());
outMaterial->SetDiffuse(inMaterial->GetDiffuse());
outMaterial->SetSpecular(inMaterial->GetSpecular());
outMaterial->SetEmissive(inMaterial->GetEmissive());
+#endif
outMaterial->SetTransparency(inMaterial->GetTransparency());
outMaterial->SetShininess(inMaterial->GetShininess());
--
GitLab