bool SocketToTF::updateTransformFromSocketTransform(zmq::message_t& message) {
socket_to_tf::TransformStamped transform;
boost::iostreams::basic_array_source<char> source((char*)message.data(), message.size());
boost::iostreams::stream<boost::iostreams::basic_array_source <char> > input_stream(source);
boost::archive::binary_iarchive ia(input_stream);
ia >> transform;
if (boost::math::isfinite(transform.x) && boost::math::isfinite(transform.y) && boost::math::isfinite(transform.z) &&
boost::math::isfinite(transform.qx) && boost::math::isfinite(transform.qy) && boost::math::isfinite(transform.qz) && boost::math::isfinite(transform.qw)) {
transform_stamped_.transform.translation.x = transform.x;
transform_stamped_.transform.translation.y = transform.y;
transform_stamped_.transform.translation.z = transform.z;
transform_stamped_.transform.rotation.x = transform.qx;
transform_stamped_.transform.rotation.y = transform.qy;
transform_stamped_.transform.rotation.z = transform.qz;
transform_stamped_.transform.rotation.w = transform.qw;
transform_stamped_.child_frame_id = transform.source_frame;
transform_stamped_.header.frame_id = transform.target_frame;
transform_stamped_.header.stamp.sec = transform.timestamp_seconds;
transform_stamped_.header.stamp.nsec = transform.timestamp_nanoseconds;
std::stringstream ss_data;
ss_data << "{ " << transform.x << " " << transform.y << " " << transform.z << " " << transform.qx << " " << transform.qy << " " << transform.qz << " " << transform.qw << " " << transform.source_frame << " " << transform.target_frame << " }";
std::string transform_data = ss_data.str();
ROS_INFO_STREAM("Received message with size " << message.size() << ": " << transform_data);
return true;
}
return false;
}
/**
* Compare message contents to specified memory region.
* @return like @c memcmp
*/
ZMQMESSAGE_DLL_PUBLIC
inline
int
msgcmp(zmq::message_t& message, const char* str, size_t len)
{
int ret = memcmp(message.data(), str, std::min(message.size(), len));
return ret ? ret : message.size() - len;
}
/**
* Compare message contents to specified memory region.
* @return like @c memcmp
*/
ZMQMESSAGE_DLL_PUBLIC
inline
int
msgcmp(zmq::message_t& message1, zmq::message_t& message2)
{
int ret = memcmp(message1.data(), message2.data(),
std::min(message1.size(), message2.size()));
return ret ? ret : message1.size() - message2.size();
}
request request::parse(zmq::message_t& msg) {
request req;
std::string result(static_cast<const char *>(msg.data()), msg.size());
std::vector<std::string> results = utils::split(result, " ", 3);
req.sender = results[0];
req.conn_id = results[1];
req.path = results[2];
std::string body;
std::string ign;
req.headers = utils::parse_json(utils::parse_netstring(results[3], body));
req.body = utils::parse_netstring(body, ign);
//check disconnect flag
req.disconnect = false;
for (std::vector<header>::const_iterator it = req.headers.begin();
it != req.headers.end(); ++it) {
if (it->first == "METHOD" && it->second == "JSON" &&
req.body == "{\"type\":\"disconnect\"}") {
req.disconnect = true;
break;
}
}
return req;
}
inline T
get_string(zmq::message_t& message, size_t limit)
{
return T(
static_cast<char*>(message.data()), std::min(message.size(), limit)
);
}
inline
T
get(zmq::message_t& message,
typename Private::EnableIf<Private::IsStr<T>::value>::type* = 0)
{
return T(static_cast<char*>(message.data()), message.size());
}
ZMQMESSAGE_DLL_PUBLIC
inline
T
get_string(zmq::message_t& message)
{
return T(static_cast<char*>(message.data()), message.size());
}
/**
* Return memory region as RawMessage. No copying takes place.
*/
ZMQMESSAGE_DLL_PUBLIC
inline
RawMessage
get_raw(zmq::message_t& message)
{
return RawMessage(message.data(), message.size());
}
Msg(const zmq::message_t &message) {
msgpack::unpacked msg;
msgpack::unpack(&msg, (const char *)message.data(), message.size());
msgpack::object obj = msg.get();
obj.convert(&data);
}
static bool unpack(zmq::message_t& message, std::string& value) {
value.assign(
static_cast<const char*>(message.data()),
message.size()
);
return true;
}
ZMQMESSAGE_DLL_PUBLIC
inline
void
get(zmq::message_t& message, T& t,
typename Private::EnableIf<Private::IsStr<T>::value>::type* = 0)
{
t = T(static_cast<char*>(message.data()), message.size());
}
bool SocketToTF::updateTransformFromSocketPointTranslation(zmq::message_t& message) {
socket_to_tf::PointTranslation point_translation;
if (use_boost_to_parse_point_translation_message_) {
boost::iostreams::basic_array_source<char> source((char*)message.data(), message.size());
boost::iostreams::stream<boost::iostreams::basic_array_source <char> > input_stream(source);
boost::archive::binary_iarchive ia(input_stream);
ia >> point_translation;
} else {
inline
void
get(zmq::message_t& message, T& t,
typename Private::DisableIf<Private::IsStr<T>::value>::type* = 0,
typename Private::DisableIf<Private::IsRaw<T>::value>::type* = 0)
{
std::stringstream ss;
ss.write(static_cast<char*>(message.data()), message.size());
ss >> t;
}
static inline
void
unpack(/* const */ zmq::message_t& message,
std::string& value)
{
value.assign(
static_cast<const char*>(message.data()),
message.size()
);
}
static bool recv_zmq_message(zmq::socket_t& sock,
zmq::message_t& msg,
T& object,
int flags = ZMQ_NOBLOCK)
{
if (!sock.recv(&msg, flags)) {
return false;
}
memcpy(&object, msg.data(), msg.size());
return true;
}
ZMQMESSAGE_DLL_PUBLIC
inline
T
get(zmq::message_t& message,
typename Private::DisableIf<Private::IsStr<T>::value>::type* = 0,
typename Private::DisableIf<Private::IsRaw<T>::value>::type* = 0)
{
std::stringstream ss;
ss.write(static_cast<char*>(message.data()), message.size());
T t = T();
ss >> t;
return t;
}
bool
nutils::recv_zmq_message(zmq::socket_t& sock,
zmq::message_t& msg,
std::string& str,
int flags)
{
if (!sock.recv(&msg, flags)) {
return false;
}
str.clear();
str.append(reinterpret_cast<char*>(msg.data()), msg.size());
return true;
}
Request::Request(const zmq::message_t& message) : m_valid(false)
{
std::string in(static_cast<const char*>(message.data()), message.size());
std::istringstream stream(in);
std::string path;
std::string headers;
// Decode message
stream >> m_sender >> m_connectionId;
stream >> path;
headers = utility::readNetString(stream);
m_body = utility::readNetString(stream);
// Decode path
if(!utility::decodeUrl(path, m_path)) {
// TODO: Throw exception
m_path = path;
}
// Parse http header
Json::Reader reader;
if ( !reader.parse(headers, m_headers) )
{
// TODO: Throw exception
}
// Parse json body
if(m_headers.isMember("METHOD") && m_headers["METHOD"] == "JSON") {
if ( !reader.parse(m_body, m_jsonBody) )
{
// TODO: Throw exception
}
}
// Parse query parameters
std::string query = m_headers.get("QUERY", std::string()).asString();
if(!query.empty()) {
std::string decodedQuery;
if(utility::decodeUrl(query, decodedQuery)) {
utility::parseQuery(decodedQuery, m_query);
}
else {
// TODO: Throw exception
}
}
m_valid = true;
}
bool
nutils::recv_zmq_message(zmq::socket_t& sock,
zmq::message_t& msg,
msgpack::object& obj,
int flags)
{
if (!sock.recv(&msg, flags)) {
return false;
}
msgpack::unpacked unpacked;
msgpack::unpack(&unpacked, reinterpret_cast<const char*>(msg.data()), msg.size());
obj = unpacked.get();
return true;
}
void
blastbeat_t::on_body(const std::string& sid,
zmq::message_t& body)
{
stream_map_t::iterator it(
m_streams.find(sid)
);
if(it == m_streams.end()) {
COCAINE_LOG_WARNING(m_log, "received an unknown session body");
return;
}
try {
it->second->write(
static_cast<const char*>(body.data()),
body.size()
);
} catch(const cocaine::error_t& e) {
COCAINE_LOG_ERROR(m_log, "unable to push a body chunk to a session - %s", e.what());
}
}
void
blastbeat_t::on_uwsgi(const std::string& sid,
zmq::message_t& message)
{
std::shared_ptr<blastbeat_stream_t> upstream(
std::make_shared<blastbeat_stream_t>(
*this,
sid
)
);
stream_map_t::iterator it;
try {
std::tie(it, std::ignore) = m_streams.emplace(
sid,
engine().enqueue(api::event_t(m_event), upstream)
);
} catch(const cocaine::error_t& e) {
COCAINE_LOG_ERROR(m_log, "unable to enqueue an event - %s", e.what());
return;
}
std::map<
std::string,
std::string
> env;
const char * ptr = static_cast<const char*>(message.data()),
* const end = ptr + message.size();
// NOTE: Skip the uwsgi header, as we already know the message
// length and we don't need uwsgi modifiers.
ptr += sizeof(uwsgi_header_t);
// Parse the HTTP headers.
while(ptr < end) {
const uint16_t * ksz,
* vsz;
ksz = reinterpret_cast<const uint16_t*>(ptr);
ptr += sizeof(*ksz);
std::string key(ptr, *ksz);
ptr += *ksz;
vsz = reinterpret_cast<const uint16_t*>(ptr);
ptr += sizeof(*vsz);
std::string value(ptr, *vsz);
ptr += *vsz;
env[key] = value;
}
// Serialize the headers.
msgpack::sbuffer buffer;
msgpack::packer<msgpack::sbuffer> packer(buffer);
packer << env;
try {
it->second->write(
buffer.data(),
buffer.size()
);
} catch(const cocaine::error_t& e) {
COCAINE_LOG_ERROR(m_log, "unable to push headers to a session - %s", e.what());
}
}
void readReply(zmq::message_t &recMsg){
// cfile is a c file descriptor (not to be confused with a protobuf FileDescriptor object)
int cfile = open("allProto.desc", O_RDONLY);
FileDescriptorSet fds;
// Parse a FileDescriptorSet object directly from the file
// Has the format of a protobuf Message - subclass FileDescriptorSet, defined in <google/protobuf/descriptor.pb.h>
// https://developers.google.com/protocol-buffers/docs/reference/cpp/google.protobuf.descriptor.pb#FieldOptions_CType.details
fds.ParseFromFileDescriptor(cfile);
// Use FileDescriptorSet method to print to screen
// fds.SerializeToOstream(&cout);
close(cfile);
// A DescriptorPool is required: provides methods to identify and manage message types at run-time
// DescriptorPool can be populated from a SimpleDescriptorDatabase, which can be populated with FileDescriptorProto objects
SimpleDescriptorDatabase sddb;
for ( int i = 0; i < fds.file_size() ; i++ ){
//Iterate over the "file" collection in the FileDescriptorSet
//Populate the sddb
sddb.Add(fds.file(i));
}
// Now construct the DescriptorPool
DescriptorPool dp(&sddb);
// DynamicMessageFactory is constucted from a populated DescriptorPool
// DescriptorPool, Descriptor, FieldDescriptor etc.: see descriptor.h -
// https://developers.google.com/protocol-buffers/docs/reference/cpp/google.protobuf.descriptor
DynamicMessageFactory dmf(&dp);
const Descriptor* desc;
const Descriptor* payload_desc;
desc = dp.FindMessageTypeByName("DescribedMessage");
// Example of dynamically creating a message from a Descriptor, retrieved by name string
Message *msg = dmf.GetPrototype(desc)->New();
msg->ParseFromArray(recMsg.data(),recMsg.size());
// Messages with required fields - Need populated.
// Requires FieldDescriptor objects to access
const FieldDescriptor* nameField = desc->FindFieldByName("full_name");
const FieldDescriptor* dataField = desc->FindFieldByName("message");
// Reflection object provides R/W access to dynamic message fields, using FieldDescriptors
// https://developers.google.com/protocol-buffers/docs/reference/cpp/google.protobuf.message#Message.GetReflection.details
// Good example of Reflection at top of that page
const Reflection *msgRefl = msg->GetReflection();
// Make payload message
payload_desc = dp.FindMessageTypeByName(msgRefl->GetString(*msg, nameField));
Message *payload_msg = dmf.GetPrototype(payload_desc)->New();
payload_msg->ParseFromString(msgRefl->GetString( *msg, dataField));
// Reflection of payload message
const Reflection *main_msgRefl = payload_msg->GetReflection();
// Payload fielddescriptors
const FieldDescriptor* main_debugField = payload_desc->FindFieldByName("debug");
main_msgRefl->SetString (payload_msg,main_debugField,"Read");
std::cout << "Payload Read" << endl;
// Put the payload data back into the envelope
string payload_data;
payload_msg->SerializeToString(&payload_data);
msgRefl->SetString(msg, dataField, payload_data);
// Now that required fields are populated, the dynamic message can be serialized and printed out.
string data;
msg->SerializeToString(&data);
// put data back into message to be replied
memcpy(recMsg.data(), data.c_str(), data.length());
// Useful examples of dynamic protobuf logic here : http://www.programmershare.com/2803644/
// (English not very good)
// 3.4 also describes dynamic compilation Uses google::protobuf::compiler Importer class
// Another link : dynamic stuff :
// http://stackoverflow.com/questions/11996557/how-to-dynamically-build-a-new-protobuf-from-a-set-of-already-defined-descriptor
// https://bitbucket.org/g19fanatic/prototest/src/dfd73a577dcc9bb51cbb3e99319cad352bfc60a8/src/main.cpp?at=master&fileviewer=file-view-default
}
inline std::string msg_to_string(zmq::message_t &message) {
return std::string(reinterpret_cast<char*>(message.data()), message.size());
}
FrameMessagesTupleListPointer FrameDecoder::decode(zmq::message_t& message, zmq::message_t* identifier)
{
FrameMessagesTupleListPointer messagesPointer(new FrameMessagesTupleList());
FrameMessagesTuplePointer messages;
Frame frame;
bool parseResult = frame.ParseFromArray(message.data(), static_cast<int>(message.size()));
if(!parseResult)
{
FILE_LOG(logWARNING)<<"Decoding of message failed. Ignoring";
return messagesPointer;
}
bool isAtomic = frame.has_identifier();
if(isAtomic)
{
messages.reset(new FrameMessagesTuple());
if(identifier)
get<1>(*messages).copy(identifier);
get<2>(*messages) = FrameMessagesPointer(new FrameMessages);
messagesPointer->push_back(messages);
}
for(int i = 0; i < frame.item_size(); i++)
{
parseResult = false;
string id;
FrameMessage* currentMessage = nullptr;
if(!isAtomic)
{
messages.reset(new FrameMessagesTuple());
if(identifier)
get<1>(*messages).copy(identifier);
get<2>(*messages) = FrameMessagesPointer(new FrameMessages);
messagesPointer->push_back(messages);
}
FrameItem currentFrameItem = frame.item(i);
if(currentFrameItem.type() == FrameItem_Type_CREATE_STORE)
{
CreateStoreOperation* createStoreOperation = new CreateStoreOperation();
parseResult = createStoreOperation->ParseFromString(currentFrameItem.data());//TODO:check return type
currentMessage = createStoreOperation;
}
else if(currentFrameItem.type() == FrameItem_Type_DROP_STORE)
{
DropStoreOperation* dropStoreOperation = new DropStoreOperation();
parseResult = dropStoreOperation->ParseFromString(currentFrameItem.data());
currentMessage = dropStoreOperation;
}
else if(currentFrameItem.type() == FrameItem_Type_SERVER_RESPONSE)
{
Result* serverResponse = new Result();
parseResult = serverResponse->ParseFromString(currentFrameItem.data());
currentMessage = serverResponse;
}
else if(currentFrameItem.type() == FrameItem_Type_SET)
{
SetOperation* setOperation = new SetOperation();
parseResult = setOperation->ParseFromString(currentFrameItem.data());
currentMessage = setOperation;
}
else if(currentFrameItem.type() == FrameItem_Type_DELETE)
{
DeleteOperation* deleteOperation = new DeleteOperation();
parseResult = deleteOperation->ParseFromString(currentFrameItem.data());
currentMessage = deleteOperation;
}
else if(currentFrameItem.type() == FrameItem_Type_GET)
{
GetOperation* getOperation = new GetOperation();
parseResult = getOperation->ParseFromString(currentFrameItem.data());
currentMessage = getOperation;
}
if(!parseResult)
{
FILE_LOG(logWARNING)<<"Unknown message received. Ignoring";
continue;
}
get<2>(*messages)->push_back(shared_ptr<FrameMessage>(currentMessage));
if(isAtomic)
id = frame.identifier();
else
get<0>(*messages) = currentFrameItem.identifier();
}
if(isAtomic)
get<0>(*messages) = frame.identifier();
return messagesPointer;
}
inline T
get_string(zmq::message_t& message)
{
return T(static_cast<char*>(message.data()), message.size());
}
/**
* Return memory region as RawMessage. No copying takes place.
*/
inline RawMessage
get_raw(zmq::message_t& message)
{
return RawMessage(message.data(), message.size());
}
Json Client::parse_message(zmq::message_t &msg){
std::string rpl = std::string(static_cast<char*>(msg.data()), msg.size());
std::string err;
return Json::parse(rpl, err);
}
/** Unpacks a message from a zeromq message */
static Message unpack(const zmq::message_t &msg) {
auto begin = reinterpret_cast<const capnp::word*>(msg.data()); // NOLINT
auto end = reinterpret_cast<const capnp::word*>(static_cast<const char*>(msg.data()) + msg.size()); // NOLINT
kj::ArrayPtr<const capnp::word> ptr(begin, end);
capnp::FlatArrayMessageReader msg_reader(ptr);
capnqml::Message ret;
auto reader = msg_reader.getRoot<serialize::Message>();
ret.setType(static_cast<Message::Type>(reader.getType()));
ret.setEndpoint(reader.getEndpoint().cStr());
ret.setExpectsAnswer(reader.getExpectsAnswer());
ret.setId(reader.getId());
ret.setOriginator(reader.getOriginator());
ret.setSchema(reader.getSchema().cStr());
auto payload = reader.getPayload();
Message::Payload data(payload.begin(), payload.end());
ret.setData(std::move(data));
return ret;
}
error server_control_executor::process_operation(
const zmq::message_t& _msg,
std::string& _output ) {
if ( _msg.size() <= 0 ) {
return SUCCESS();
}
int port = 0, num_hash_rounds = 0;
buffer_crypt::array_t shared_secret;
std::string encryption_algorithm;
error ret = get_server_properties(
port_prop_,
port,
num_hash_rounds,
shared_secret,
encryption_algorithm );
if ( !ret.ok() ) {
irods::log( PASS( ret ) );
return PASS( ret );
}
// decrypt the message before passing to avro
buffer_crypt crypt(
shared_secret.size(), // key size
0, // salt size ( we dont send a salt )
num_hash_rounds, // num hash rounds
encryption_algorithm.c_str() );
buffer_crypt::array_t iv;
buffer_crypt::array_t data_to_process;
const uint8_t* data_ptr = static_cast< const uint8_t* >( _msg.data() );
buffer_crypt::array_t data_to_decrypt(
data_ptr,
data_ptr + _msg.size() );
ret = crypt.decrypt(
shared_secret,
iv,
data_to_decrypt,
data_to_process );
if ( !ret.ok() ) {
irods::log( PASS( ret ) );
return PASS( ret );
}
std::auto_ptr<avro::InputStream> in = avro::memoryInputStream(
static_cast<const uint8_t*>(
data_to_process.data() ),
data_to_process.size() );
avro::DecoderPtr dec = avro::binaryDecoder();
dec->init( *in );
control_plane_command cmd;
avro::decode( *dec, cmd );
std::string cmd_name, cmd_option, wait_option;
host_list_t cmd_hosts;
size_t wait_seconds = 0;
ret = extract_command_parameters(
cmd,
cmd_name,
cmd_option,
wait_option,
wait_seconds,
cmd_hosts );
if ( !ret.ok() ) {
irods::log( PASS( ret ) );
return PASS( ret );
}
// add safeguards - if server is paused only allow a resume call
server_state& s = server_state::instance();
std::string the_server_state = s();
if ( server_state::PAUSED == the_server_state &&
SERVER_CONTROL_RESUME != cmd_name ) {
_output = SERVER_PAUSED_ERROR;
return SUCCESS();
}
// the icat needs to be notified first in certain
// cases such as RESUME where it is needed to capture
// the host list for validation, etc
ret = notify_icat_and_local_servers_preop(
cmd_name,
cmd_option,
wait_option,
wait_seconds,
cmd_hosts,
_output );
if ( !ret.ok() ) {
irods::log( PASS( ret ) );
return PASS( ret );
}
host_list_t irods_hosts;
ret = get_resource_host_names(
irods_hosts );
if ( !ret.ok() ) {
irods::log( PASS( ret ) );
return PASS( ret );
}
if ( SERVER_CONTROL_ALL_OPT == cmd_option ) {
cmd_hosts = irods_hosts;
}
host_list_t valid_hosts;
ret = validate_host_list(
irods_hosts,
cmd_hosts,
valid_hosts );
if ( !ret.ok() ) {
irods::log( PASS( ret ) );
return PASS( ret );
}
ret = process_host_list(
cmd_name,
wait_option,
wait_seconds,
valid_hosts,
_output );
if ( !ret.ok() ) {
irods::log( PASS( ret ) );
return PASS( ret );
}
// the icat needs to be notified last in certain
// cases such as SHUTDOWN or PAUSE where it is
// needed to capture the host list for validation
ret = notify_icat_and_local_servers_postop(
cmd_name,
cmd_option,
wait_option,
wait_seconds,
cmd_hosts, // dont want sanitized
_output );
if ( !ret.ok() ) {
irods::log( PASS( ret ) );
return PASS( ret );
}
return ret;
} // process_operation
std::string ZmqUtilInterface::message_to_string(const zmq::message_t& message) {
return std::string(static_cast<const char*>(message.data()), message.size());
}
