From fc980a3e4cb4e6de356566fd1adf488a6c675f35 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Tue, 31 Oct 2023 14:45:06 -0700 Subject: [PATCH 01/32] first commit to comms_bridge, wip --- communications/comms_bridge/CMakeLists.txt | 88 +++++++ communications/comms_bridge/README.md | 26 ++ .../include/comms_bridge/bridge_publisher.h | 138 ++++++++++ .../include/comms_bridge/bridge_subscriber.h | 129 ++++++++++ .../comms_bridge/dds_ros_bridge_publisher.h | 45 ++++ .../comms_bridge/ros_dds_bridge_subscriber.h | 53 ++++ .../comms_bridge/ros_ros_bridge_publisher.h | 64 +++++ .../comms_bridge/ros_ros_bridge_subscriber.h | 70 ++++++ .../comms_bridge/nodelet_plugins.xml | 13 + communications/comms_bridge/package.xml | 37 +++ .../comms_bridge/src/bridge_publisher.cpp | 238 ++++++++++++++++++ .../comms_bridge/src/bridge_subscriber.cpp | 187 ++++++++++++++ .../src/dds_ros_bridge_publisher.cpp | 32 +++ .../src/ros_dds_bridge_subscriber.cpp | 45 ++++ .../src/ros_ros_bridge_publisher.cpp | 121 +++++++++ .../src/ros_ros_bridge_subscriber.cpp | 124 +++++++++ .../ff_msgs/msg/RelayAdvertisement.msg | 17 ++ communications/ff_msgs/msg/RelayContent.msg | 13 + communications/ff_msgs/msg/RelayReset.msg | 7 + communications/readme.md | 1 + scripts/git/cpplint.py | 4 +- 21 files changed, 1450 insertions(+), 2 deletions(-) create mode 100644 communications/comms_bridge/CMakeLists.txt create mode 100644 communications/comms_bridge/README.md create mode 100644 communications/comms_bridge/include/comms_bridge/bridge_publisher.h create mode 100644 communications/comms_bridge/include/comms_bridge/bridge_subscriber.h create mode 100644 communications/comms_bridge/include/comms_bridge/dds_ros_bridge_publisher.h create mode 100644 communications/comms_bridge/include/comms_bridge/ros_dds_bridge_subscriber.h create mode 100644 communications/comms_bridge/include/comms_bridge/ros_ros_bridge_publisher.h create mode 100644 communications/comms_bridge/include/comms_bridge/ros_ros_bridge_subscriber.h create mode 100644 communications/comms_bridge/nodelet_plugins.xml create mode 100644 communications/comms_bridge/package.xml create mode 100644 communications/comms_bridge/src/bridge_publisher.cpp create mode 100644 communications/comms_bridge/src/bridge_subscriber.cpp create mode 100644 communications/comms_bridge/src/dds_ros_bridge_publisher.cpp create mode 100644 communications/comms_bridge/src/ros_dds_bridge_subscriber.cpp create mode 100644 communications/comms_bridge/src/ros_ros_bridge_publisher.cpp create mode 100644 communications/comms_bridge/src/ros_ros_bridge_subscriber.cpp create mode 100644 communications/ff_msgs/msg/RelayAdvertisement.msg create mode 100644 communications/ff_msgs/msg/RelayContent.msg create mode 100644 communications/ff_msgs/msg/RelayReset.msg diff --git a/communications/comms_bridge/CMakeLists.txt b/communications/comms_bridge/CMakeLists.txt new file mode 100644 index 0000000000..6674c2479e --- /dev/null +++ b/communications/comms_bridge/CMakeLists.txt @@ -0,0 +1,88 @@ +# Copyright (c) 2017, United States Government, as represented by the +# Administrator of the National Aeronautics and Space Administration. +# +# All rights reserved. +# +# The Astrobee platform is licensed under the Apache License, Version 2.0 +# (the "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. + + +cmake_minimum_required(VERSION 3.0) +project(comms_bridge) + +## Compile as C++14, supported in ROS Kinetic and newer +add_compile_options(-std=c++14) + +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + nodelet + message_generation + message_runtime + image_transport + std_msgs + sensor_msgs + ff_msgs + topic_tools +) + +catkin_package( + LIBRARIES src + CATKIN_DEPENDS + message_runtime + std_msgs +) + +########### +## Build ## +########### + +# Specify additional locations of header files +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + + +# Declare C++ libraries +add_library(comms_bridge_sub + src/bridge_subscriber.cpp + src/ros_dds_bridge_subscriber.cpp +) +add_dependencies(comms_bridge_sub ${catkin_EXPORTED_TARGETS}) +target_link_libraries(comms_bridge_sub + ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${CMAKE_DL_LIBS} ) + + +add_library(comms_bridge_pub + src/bridge_publisher.cpp + src/dds_ros_bridge_publisher.cpp +) +add_dependencies(comms_bridge_pub ${catkin_EXPORTED_TARGETS} ) +target_link_libraries(comms_bridge_pub + ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${CMAKE_DL_LIBS} ) + + +############# +## Install ## +############# +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME}_sub ${PROJECT_NAME}_pub + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +# Mark nodelet_plugin for installation +install(FILES nodelet_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/communications/comms_bridge/README.md b/communications/comms_bridge/README.md new file mode 100644 index 0000000000..43ec478cef --- /dev/null +++ b/communications/comms_bridge/README.md @@ -0,0 +1,26 @@ +\page coms_bridge Modular Polymorphic ROS Relay + + +See that dir for architecture info. + +This is unidirectional, supporting an arbitrary number of in-out topics. +For bi-directional, an additional paired set of flipped subscribers/publishers can be run. + +This is a demonstration of unidirectional relaying of ROS messages from one topic to another, without any specification of the message types, nor even the requirement that the types be available to the nodes at compile-time. + +This is a demonstration insofar as the metadata backchannel is itself just ROS topics on the same ROS master, but this can be replaced with other conduits. + +## Basic architecture + +- The subscriber node (A) subscribes to a specified list of topics +- Upon receipt of the first message on a given forwarded topic, A sends an advertisement message to a metadata "advertisement" topic +- Receiving the metadata advertisement, the republisher node (B) itself advertises the output (republished) topic +- The first and all subsequent messages A receives are serialized and published to a metadata "content" topic +- A delay is implemented in B between its advertising and subsequent republishing of any messages to practically avoid the potential race condition of a subscriber on that end missing the first message as it connects to B only upon seeing the advertisement +- Otherwise, B deserializes all messages received on the "content" metadata topic, looks up the topic in previously received "advertisement" metadata, and republishes the message on the output topic + +## Reverse channel + +To account for situations in which the republisher node may be restarted after the subscriber node has already been running, or to handle implementations in which advertisement metadata could be lost (eg if the metadata channels were implemented on a lossy medium), the republisher can request retransmission of "advertisement" metadata for any topic. It does this by publishing a message to a "reset" metadata channel, specifying the topic name, to which the the subscriber node listens and retransmits the advertisement. + + diff --git a/communications/comms_bridge/include/comms_bridge/bridge_publisher.h b/communications/comms_bridge/include/comms_bridge/bridge_publisher.h new file mode 100644 index 0000000000..ba484e15ad --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/bridge_publisher.h @@ -0,0 +1,138 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_BRIDGE_PUBLISHER_H_ +#define COMMS_BRIDGE_BRIDGE_PUBLISHER_H_ + +/* + Virtual base class for the subscriber (input side) of a generic ROS bridge + Actual implementation depends on an inheriting class + */ + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +typedef std::shared_ptr SubscriberPtr; +typedef std::shared_ptr PublisherPtr; +typedef std::chrono::time_point timepoint_t; + +class BridgePublisher { + public: + virtual ~BridgePublisher(); + + // 0 => none, > 0 => progressively more + void setVerbosity(unsigned int verbosity); + + protected: + // a yet-unknown message type will have null string values + class AdvertisementInfo { + public: + // whehter republisher should advertise this as a latching ROS topic + bool latching; + + // ROS message data type name, eg "std_msgs/Int32" + std::string data_type; + + // ROS message md5sum of type, eg "da5909fbe378aeaf85e547e830cc1bb7" + std::string md5_sum; + + // ROS message definition, eg as one finds in a .msg definition file + std::string definition; + }; + + class ContentInfo { + public: + // the md5sum from the original advertisement + std::string type_md5_sum; + + // opaque serialized message data + std::vector data; + }; + + class RelayTopicInfo { + public: + RelayTopicInfo() : out_topic(""), advertised(false), relay_seqnum(0) {} + + ros::Publisher publisher; // our subscriber on this input topic + std::string out_topic; // topic name to republish on + bool advertised; // whether we have sent type info to other side + unsigned int relay_seqnum; // counter of messages relayed on this topic + + // Will be filled in only on receipt of advertisement from our + // bridger subscriber counterpart + AdvertisementInfo ad_info; + + std::queue + waiting_msgs; // unpublished messages because type yet unknown or we're still delaying after advertising + timepoint_t delay_until; // time to wait until relaying any data, after having advertised + }; + + /**************************************************************************/ + + // This base class should only be extended, not itself instantiated + explicit BridgePublisher(double ad2pub_delay); + + // Should be called by implementing class when it has received a topic's + // AdvertisementInfo + // Must be called with mutex held + bool advertiseTopic(const std::string& output_topic, const AdvertisementInfo& ad_info); + + // Should be called by an implementing class when it receives a serialized + // message over its particular conduit + // Must be called with mutex held + bool relayMessage(RelayTopicInfo& topic_info, const ContentInfo& content_info); + + // worker thread spawned at construction + void drainThread(); + + // does actual work of publishing a message (eg called by relayMessage) + // Must be called with mutex held + bool relayContent(RelayTopicInfo& topic_info, const ContentInfo& content_info); + + // Publishes messages that were enqueued for a given topic + // Must be called with mutex held + void drainWaitingQueue(RelayTopicInfo& topic_info); + void drainWaitingQueue(std::string const& output_topic); + + unsigned int m_verbose; + + double m_ad2pub_delay; // seconds + + std::mutex m_mutex; // serializes access to below data structures + std::unique_ptr worker_thread; + std::map m_relay_topics; // keyed by input topic + std::priority_queue> m_drain_queue; + std::condition_variable m_drain_cv; + // stats: + unsigned int m_n_relayed; +}; + +#endif // COMMS_BRIDGE_BRIDGE_PUBLISHER_H_ diff --git a/communications/comms_bridge/include/comms_bridge/bridge_subscriber.h b/communications/comms_bridge/include/comms_bridge/bridge_subscriber.h new file mode 100644 index 0000000000..0ca54395aa --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/bridge_subscriber.h @@ -0,0 +1,129 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_BRIDGE_SUBSCRIBER_H_ +#define COMMS_BRIDGE_BRIDGE_SUBSCRIBER_H_ + +/* + Virtual base class for the subscriber (input side) of a generic ROS bridge + Actual implementation depends on an inheriting class + */ + +#include + +#include +#include + +#include +#include +#include +#include + +typedef std::shared_ptr SubscriberPtr; +typedef std::shared_ptr PublisherPtr; + +class BridgeSubscriber { + public: + BridgeSubscriber(); + virtual ~BridgeSubscriber(); + + // Add a relay between this input topic on the subscriber side and this + // output topic on the republisher side + // All relays must have a unique input topic and unique output topic + // Other topologies (merging topics or duplicating topics) should be + // implemented by some other node on top of this one + bool addTopic(std::string const& in_topic, std::string const& out_topic); + + // 0 => none, > 0 => progressively more + void setVerbosity(unsigned int verbosity); + + protected: + class AdvertisementInfo { + public: + // whehter republisher should advertise this as a latching ROS topic + bool latching; + + // ROS message data type name, eg "std_msgs/Int32" + std::string data_type; + + // ROS message md5sum of type, eg "da5909fbe378aeaf85e547e830cc1bb7" + std::string md5_sum; + + // ROS message definition, eg as one finds in a .msg definition file + std::string definition; + }; + + class RelayTopicInfo { + public: + RelayTopicInfo() : out_topic(""), advertised(false), relay_seqnum(0) {} + + SubscriberPtr sub; // our subscriber on this input topic + std::string out_topic; // topic name to republish on + bool advertised; // whether we have sent type info to other side + unsigned int relay_seqnum; // counter of messages relayed on this topic + + // not filled in until advertiseTopic() is called + AdvertisementInfo ad_info; + }; + + class ContentInfo { + public: + // the md5sum from the original advertisement + std::string type_md5_sum; + + // length in bytes of the serialized data + size_t data_size; + + // opaque serialized message data + const uint8_t* data; + }; + + // Called via addTopic() + // Notifies implementation that a bridge between these two topics is needed + // No information about the message type etc. is yet available + // Called with the mutex held + virtual void subscribeTopic(std::string const& in_topic, const RelayTopicInfo& info) = 0; + + // Called upon receipt of the first message on a subscribed topic + // Notifies implementation to advertise the topic with this type info + // Called with the mutex held + virtual void advertiseTopic(const RelayTopicInfo& info) = 0; + + // Called on receipt of any message on a subscribed topic + // Notifies implementation to relay this message content to the output topic + // Note any pointers within info struct are valid only during this call + // Called with the mutex held + virtual void relayMessage(const RelayTopicInfo& topic_info, ContentInfo const& content_info) = 0; + + /**************************************************************************/ + + SubscriberPtr rosSubscribe(std::string const& topic); + + void handleRelayedMessage(const ros::MessageEvent& msg_event, + std::string const& topic, SubscriberPtr sub); + + unsigned int m_verbose; + + std::mutex m_mutex; // serializes access to below data structures + uint8_t* m_msgbuffer; // data serialization scratch + std::map m_relay_topics; // keyed by input topic + // stats: + unsigned int m_n_relayed; +}; + +#endif // COMMS_BRIDGE_BRIDGE_SUBSCRIBER_H_ diff --git a/communications/comms_bridge/include/comms_bridge/dds_ros_bridge_publisher.h b/communications/comms_bridge/include/comms_bridge/dds_ros_bridge_publisher.h new file mode 100644 index 0000000000..89d70a7c88 --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/dds_ros_bridge_publisher.h @@ -0,0 +1,45 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_DDS_ROS_BRIDGE_PUBLISHER_H_ +#define COMMS_BRIDGE_DDS_ROS_BRIDGE_PUBLISHER_H_ + +/* This is a specialization of ROSBridgePublisher using a DDS conduit +*/ + +#include +#include +#include + +#include + +// default time to delay between advertising and publishing on that topic [sec] +#define DEFAULT_DDSROSBRIDGE_PUB_ADVERTISE_DELAY 3.0 + +class DDSROSBridgePublisher : public BridgePublisher { + public: + explicit DDSROSBridgePublisher(double ad2pub_delay = DEFAULT_DDSROSBRIDGE_PUB_ADVERTISE_DELAY); + virtual ~DDSROSBridgePublisher(); + + protected: + // prohibit shallow copy or assignment + DDSROSBridgePublisher(const DDSROSBridgePublisher&) : BridgePublisher(m_ad2pub_delay) {} + void operator=(const DDSROSBridgePublisher&) {} +}; + +#endif // COMMS_BRIDGE_DDS_ROS_BRIDGE_PUBLISHER_H_ diff --git a/communications/comms_bridge/include/comms_bridge/ros_dds_bridge_subscriber.h b/communications/comms_bridge/include/comms_bridge/ros_dds_bridge_subscriber.h new file mode 100644 index 0000000000..60dccc72b6 --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/ros_dds_bridge_subscriber.h @@ -0,0 +1,53 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_ROS_DDS_BRIDGE_SUBSCRIBER_H_ +#define COMMS_BRIDGE_ROS_DDS_BRIDGE_SUBSCRIBER_H_ + +/* This is a specialization of ROSBridgeSubscriber using a DDS conduit +*/ + +#include +#include +#include + +#include + +#include + +class ROSDDSBridgeSubscriber : public BridgeSubscriber { + public: + ROSDDSBridgeSubscriber(); + virtual ~ROSDDSBridgeSubscriber(); + + // Called with the mutex held + virtual void subscribeTopic(std::string const& in_topic, const RelayTopicInfo& info); + + // Called with the mutex held + virtual void advertiseTopic(const RelayTopicInfo& info); + + // Called with the mutex held + virtual void relayMessage(const RelayTopicInfo& topic_info, ContentInfo const& content_info); + + protected: + // prohibit shallow copy or assignment + ROSDDSBridgeSubscriber(const ROSDDSBridgeSubscriber&) {} + void operator=(const ROSDDSBridgeSubscriber&) {} +}; + +#endif // COMMS_BRIDGE_ROS_DDS_BRIDGE_SUBSCRIBER_H_ diff --git a/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_publisher.h b/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_publisher.h new file mode 100644 index 0000000000..7bc0f308b9 --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_publisher.h @@ -0,0 +1,64 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_ROS_ROS_BRIDGE_PUBLISHER_H_ +#define COMMS_BRIDGE_ROS_ROS_BRIDGE_PUBLISHER_H_ + +/* This is a specialization of ROSBridgePublisher using a ROS conduit + that is another topic on the same ROS master + ie, it's a ROS-ROS bridge over ROS +*/ + +#include +#include +#include + +#include + +#include + +#define DEFAULT_ROSROSBRIDGE_PUB_META_TOPIC_PREFIX "/polymorph_relay" +// default time to delay between advertising and publishing on that topic [sec] +#define DEFAULT_ROSROSBRIDGE_PUB_ADVERTISE_DELAY 3.0 + +class ROSROSBridgePublisher : public ROSBridgePublisher { + public: + ROSROSBridgePublisher(const std::string& meta_topic_prefix = DEFAULT_ROSROSBRIDGE_PUB_META_TOPIC_PREFIX, + double ad2pub_delay = DEFAULT_ROSROSBRIDGE_PUB_ADVERTISE_DELAY); + virtual ~ROSROSBridgePublisher(); + + protected: + void setupMetaChannels(); + void setupReverseMetaChannels(); + + void requestTopicInfo(std::string const& output_topic); + + void handleContentMessage(const ros_bridge::RelayContent::ConstPtr& msg); + void handleAdMessage(const ros_bridge::RelayAdvertisement::ConstPtr& msg); + + std::string m_meta_topic_prefix; + ros::Subscriber m_advert_sub; + ros::Subscriber m_content_sub; + ros::Publisher m_reset_pub; + + // prohibit shallow copy or assignment + ROSROSBridgePublisher(const ROSROSBridgePublisher&) : ROSBridgePublisher(m_ad2pub_delay) {} + void operator=(const ROSROSBridgePublisher&) {} +}; + +#endif // COMMS_BRIDGE_ROS_ROS_BRIDGE_PUBLISHER_H_ diff --git a/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_subscriber.h b/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_subscriber.h new file mode 100644 index 0000000000..854da6d709 --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_subscriber.h @@ -0,0 +1,70 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_ROS_ROS_BRIDGE_SUBSCRIBER_H_ +#define COMMS_BRIDGE_ROS_ROS_BRIDGE_SUBSCRIBER_H_ + +/* This is a specialization of ROSBridgeSubscriber using a ROS conduit + that is another topic on the same ROS master + ie, it's a ROS-ROS bridge over ROS +*/ + +#include +#include +#include + +#include + +#include +#include + +#define DEFAULT_ROSROSBRIDGE_PUB_META_TOPIC_PREFIX "/polymorph_relay" + +class ROSROSBridgeSubscriber : public ROSBridgeSubscriber { + public: + explicit ROSROSBridgeSubscriber(const std::string& meta_topic_prefix = DEFAULT_ROSROSBRIDGE_PUB_META_TOPIC_PREFIX); + virtual ~ROSROSBridgeSubscriber(); + + // Called with the mutex held + virtual void subscribeTopic(std::string const& in_topic, const RelayTopicInfo& info); + + // Called with the mutex held + virtual void advertiseTopic(const RelayTopicInfo& info); + + // Called with the mutex held + virtual void relayMessage(const RelayTopicInfo& topic_info, ContentInfo const& content_info); + + protected: + void setupMetaChannels(); + void setupReverseMetaChannels(); + void handleResetMessage(const ros_bridge::RelayReset::ConstPtr& msg); + + std::string m_meta_topic_prefix; + unsigned int m_n_advertised; + ros::Publisher m_advertiser_pub; + ros::Publisher m_relayer_pub; + ros::Subscriber m_reset_sub; + + std::map requested_resets; // topics requiring re-advert + + // prohibit shallow copy or assignment + ROSROSBridgeSubscriber(const ROSROSBridgeSubscriber&) {} + void operator=(const ROSROSBridgeSubscriber&) {} +}; + +#endif // COMMS_BRIDGE_ROS_ROS_BRIDGE_SUBSCRIBER_H_ diff --git a/communications/comms_bridge/nodelet_plugins.xml b/communications/comms_bridge/nodelet_plugins.xml new file mode 100644 index 0000000000..b7729ef427 --- /dev/null +++ b/communications/comms_bridge/nodelet_plugins.xml @@ -0,0 +1,13 @@ + + + + Nodelet for the polymorph relay subscriber + + + + + Nodelet for the polymorph relay publisher + + + \ No newline at end of file diff --git a/communications/comms_bridge/package.xml b/communications/comms_bridge/package.xml new file mode 100644 index 0000000000..aff3ef03bc --- /dev/null +++ b/communications/comms_bridge/package.xml @@ -0,0 +1,37 @@ + + + comms_bridge + 0.0.0 + + The comms_bridge package + + + Apache License, Version 2.0 + + + Astrobee Flight Software + + + Astrobee Flight Software + + catkin + roscpp + nodelet + message_generation + message_runtime + image_transport + std_msgs + sensor_msgs + ff_msgs + topic_tools + roscpp + nodelet + message_generation + message_runtime + image_transport + std_msgs + sensor_msgs + ff_msgs + topic_tools + + diff --git a/communications/comms_bridge/src/bridge_publisher.cpp b/communications/comms_bridge/src/bridge_publisher.cpp new file mode 100644 index 0000000000..4f4d0d63bf --- /dev/null +++ b/communications/comms_bridge/src/bridge_publisher.cpp @@ -0,0 +1,238 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "comms_bridge/bridge_publisher.h" + +#include +#include +#include +#include +#include + +// ROS internal publisher message queue size +#define PUBLISHER_QUEUE_SIZE 10 +// max number of messages to store because we're not ready to republish them +#define MAX_WAITING_QUEUE_SIZE 100 +// default time to delay between advertising and publishing on that topic [sec] +#define DEFAULT_ADVERTISE_DELAY 3.0 + +using ros::Publisher; +using ros::Publisher; +using topic_tools::ShapeShifter; + +BridgePublisher::BridgePublisher(double ad2pub_delay) : m_ad2pub_delay(ad2pub_delay) { + m_n_relayed = 0; + m_verbose = 0; + + worker_thread = std::unique_ptr(new std::thread(&BridgePublisher::drainThread, this)); +} + +BridgePublisher::~BridgePublisher() {} + +void BridgePublisher::setVerbosity(unsigned int verbosity) { m_verbose = verbosity; } + +bool BridgePublisher::advertiseTopic(const std::string& output_topic, const AdvertisementInfo& ad_info) { + if (m_verbose) + printf("Advertising for topic %s\n", output_topic.c_str()); + + std::map::iterator iter = m_relay_topics.find(output_topic); + if (iter == m_relay_topics.end()) { + if (m_verbose) + printf(" topic is yet-unknown\n"); + RelayTopicInfo topic_info; + topic_info.out_topic = output_topic; + iter = m_relay_topics.emplace(output_topic, topic_info).first; + } + + RelayTopicInfo &topic_info = iter->second; + + if (topic_info.advertised) { + // we've already advertised, nothing to do + if (topic_info.ad_info.md5_sum != ad_info.md5_sum || + topic_info.ad_info.data_type != ad_info.data_type || + topic_info.ad_info.definition != ad_info.definition) + ROS_ERROR("Received advertisement with differing type info from previous advertisement, messages will be lost"); + return false; + } else { + if (topic_info.ad_info.md5_sum != "" && topic_info.ad_info.md5_sum != ad_info.md5_sum) + ROS_WARN( + "Advertisement received for topic %s with differing md5sum from prior content, prior messages will be lost", + output_topic.c_str()); + + // we haven't seen an ad for this topic yet, populate everything + topic_info.ad_info = ad_info; + + ros::NodeHandle nh; + ShapeShifter shifter; + // will always succeed, even if inconsistent type info + shifter.morph(ad_info.md5_sum, ad_info.data_type, ad_info.definition, ""); + topic_info.publisher = shifter.advertise(nh, output_topic, PUBLISHER_QUEUE_SIZE, ad_info.latching); + + topic_info.advertised = true; + + const timepoint_t now = std::chrono::steady_clock::now(); + topic_info.delay_until = now + std::chrono::microseconds((unsigned)(m_ad2pub_delay * 1e6)); + + if (m_ad2pub_delay > 0) { + m_drain_queue.push(std::make_pair(topic_info.delay_until, output_topic)); + m_drain_cv.notify_all(); + } else { + // no post-advertise delay + + // send out any old messages first + drainWaitingQueue(topic_info); + } + } + + return true; +} + +bool BridgePublisher::relayContent(RelayTopicInfo& topic_info, const ContentInfo& content_info) { + ShapeShifter shifter; + + // IStreams don't really modify their input but the implementation is + // lazily general so we have to be ugly + ros::serialization::IStream stream(const_cast(content_info.data.data()), content_info.data.size()); + try { + stream.next(shifter); + } + catch (ros::Exception &e) { + ROS_ERROR("Deserialization error (%s), losing message", e.what()); + return false; + } + + // will always succeed, even if inconsistent type info + shifter.morph(topic_info.ad_info.md5_sum, topic_info.ad_info.data_type, topic_info.ad_info.definition, ""); + + try { + topic_info.publisher.publish(shifter); + } + catch (ros::Exception &e) { + ROS_ERROR("Error publishing morphed message (%s), losing message", e.what()); + return false; + } + + topic_info.relay_seqnum++; + m_n_relayed++; + + if (m_verbose) + printf("Sent message on topic %s\n", topic_info.out_topic.c_str()); + + return true; +} + +bool BridgePublisher::relayMessage(RelayTopicInfo& topic_info, const ContentInfo& content_info) { + if (m_verbose) + printf("Relaying content message for topic %s\n", topic_info.out_topic.c_str()); + + if (!topic_info.advertised) { + if (m_verbose) + printf(" topic is yet-unknown, enqueuing message for now\n"); + + topic_info.waiting_msgs.emplace(content_info); + + return true; + } + + if (topic_info.ad_info.md5_sum != content_info.type_md5_sum) + ROS_WARN( + "Message received for topic %s with differing md5sum from prior advertiesment, future messages will be lost", + topic_info.out_topic.c_str()); + + const timepoint_t now = std::chrono::steady_clock::now(); + + if (!topic_info.advertised || now < topic_info.delay_until) { + // we still don't have type information for this message, or + // we're still delaying after advertising + // so just enqueue messages + + if (m_verbose) + printf(" topic is not %s, enqueuing message for now\n", + (!topic_info.advertised ? "yet advertised" : "yet ready")); + + // limit the number of waiting messages, tossing the oldest ones + while (topic_info.waiting_msgs.size() >= MAX_WAITING_QUEUE_SIZE) + topic_info.waiting_msgs.pop(); + + topic_info.waiting_msgs.emplace(content_info); + + if (m_verbose > 1) + printf(" %zu messages now waiting\n", topic_info.waiting_msgs.size()); + + return true; + } + + // send out any old messages first + drainWaitingQueue(topic_info); + + // send out this message + return relayContent(topic_info, content_info); +} + +void BridgePublisher::drainThread() { + std::unique_lock lk(m_mutex); + + if (m_verbose) + printf("Started drain thread\n"); + + while (1) { + while (m_drain_queue.empty()) + m_drain_cv.wait(lk); + + std::pair entry = m_drain_queue.top(); + m_drain_queue.pop(); + + lk.unlock(); + + timepoint_t t = entry.first; + + if (m_verbose > 1) + printf( + "draining %s in %0.3f seconds\n", entry.second.c_str(), + std::chrono::duration((t - std::chrono::steady_clock::now())).count()); + std::this_thread::sleep_until(t); + + if (m_verbose > 1) + printf("draining %s now\n", entry.second.c_str()); + + lk.lock(); + + drainWaitingQueue(entry.second); + } +} + +// called with mutex held +void BridgePublisher::drainWaitingQueue(RelayTopicInfo& topic_info) { + if (m_verbose && topic_info.waiting_msgs.size() > 0) + printf("Draining queue of %zu waiting messages\n", topic_info.waiting_msgs.size()); + while (topic_info.waiting_msgs.size() > 0) { + const ContentInfo old_msg = topic_info.waiting_msgs.front(); + relayContent(topic_info, old_msg); + topic_info.waiting_msgs.pop(); + } +} + +// called with mutex held +void BridgePublisher::drainWaitingQueue(std::string const& output_topic) { + std::map::iterator iter = m_relay_topics.find(output_topic); + assert(iter != m_relay_topics.end()); + + RelayTopicInfo &topic_info = iter->second; + + drainWaitingQueue(topic_info); +} diff --git a/communications/comms_bridge/src/bridge_subscriber.cpp b/communications/comms_bridge/src/bridge_subscriber.cpp new file mode 100644 index 0000000000..23ca60a633 --- /dev/null +++ b/communications/comms_bridge/src/bridge_subscriber.cpp @@ -0,0 +1,187 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include + +#include "comms_bridge/bridge_subscriber.h" + +// ROS internal subscriber message queue size +#define SUBSCRIBER_QUEUE_SIZE 10 +// max serialized message size +#define ROS_BRIDGE_MAX_MSG_SIZE (10*1024*1024) + +using ros::Subscriber; +using ros::Publisher; +using topic_tools::ShapeShifter; + +BridgeSubscriber::BridgeSubscriber() { + m_n_relayed = 0; + m_verbose = 0; + + m_msgbuffer = new uint8_t[ROS_BRIDGE_MAX_MSG_SIZE]; +} + +BridgeSubscriber::~BridgeSubscriber() { delete[] m_msgbuffer; } + +void BridgeSubscriber::setVerbosity(unsigned int verbosity) { m_verbose = verbosity; } + +void BridgeSubscriber::handleRelayedMessage(const ros::MessageEvent& msg_event, + std::string const& topic, SubscriberPtr sub) { + ShapeShifter::ConstPtr ptr = msg_event.getConstMessage(); + + boost::shared_ptr const& connection_header = + msg_event.getConnectionHeaderPtr(); + + if (m_verbose) { + printf("got data on %s:\n", topic.c_str()); + if (m_verbose > 1) { + printf(" datatype: \"%s\"\n", ptr->getDataType().c_str()); + printf(" md5: \"%s\"\n", ptr->getMD5Sum().c_str()); + } + if (m_verbose > 2) + printf(" def: \"%s\"\n", ptr->getMessageDefinition().c_str()); + + if (m_verbose > 2) { + printf(" conn header:\n"); + for (ros::M_string::const_iterator iter = connection_header->begin(); + iter != connection_header->end(); + iter++) + printf(" %s: %s\n", iter->first.c_str(), iter->second.c_str()); + } + } + + std::unique_lock lock(m_mutex); + + std::map::iterator iter = m_relay_topics.find(topic); + if (iter == m_relay_topics.end()) { + printf("Received message on non-relayed topic %s\n", topic.c_str()); + return; + } + + RelayTopicInfo &topic_info = iter->second; + + if (!topic_info.advertised) { + if (m_verbose) + printf(" sending advertisement\n"); + + bool latching = false; + if (connection_header) { + ros::M_string::const_iterator iter = connection_header->find("latching"); + if (iter != connection_header->end() && iter->second == "1") + latching = true; + } + + AdvertisementInfo ad_info; + ad_info.latching = latching; + ad_info.data_type = ptr->getDataType(); + ad_info.md5_sum = ptr->getMD5Sum(); + ad_info.definition = ptr->getMessageDefinition(); + topic_info.ad_info = ad_info; + + advertiseTopic(topic_info); + topic_info.advertised = true; + + // ROS "latches" the msg type md5sum for us, only accepting same in future + // so no need for us to save and check that it doesn't change + } + + ros::serialization::OStream stream(m_msgbuffer, ROS_BRIDGE_MAX_MSG_SIZE); + stream.next(*ptr); // serializes just the message contents + ssize_t serialized_size = ROS_BRIDGE_MAX_MSG_SIZE - stream.getLength(); + if (m_verbose > 2) + printf(" serialized size = %zd\n", serialized_size); + if (serialized_size <= 0) { + ROS_ERROR("Serialization buffer size deficient, discarding message"); + return; + } + + ContentInfo content_info; + content_info.type_md5_sum = ptr->getMD5Sum(); + content_info.data_size = (size_t)serialized_size; + content_info.data = m_msgbuffer; + relayMessage(topic_info, content_info); + topic_info.relay_seqnum++; + m_n_relayed++; + + lock.release()->unlock(); + // now done with any access to topic info + + if (m_verbose) + fflush(stdout); +} + +// called with mutex held +SubscriberPtr BridgeSubscriber::rosSubscribe(std::string const& topic) { + ros::NodeHandle nh; + + SubscriberPtr ptr = std::make_shared(); + + ros::SubscribeOptions opts; + opts.topic = topic; + opts.queue_size = SUBSCRIBER_QUEUE_SIZE; + opts.md5sum = ros::message_traits::md5sum(); + opts.datatype = ros::message_traits::datatype(); + opts.helper = boost::make_shared &> >( + std::bind(&BridgeSubscriber::handleRelayedMessage, this, std::placeholders::_1, topic, ptr)); + *ptr = nh.subscribe(opts); + + if (m_verbose) + printf("Subscribed to topic %s\n", topic.c_str()); + + return ptr; +} + +bool BridgeSubscriber::addTopic(std::string const& in_topic, std::string const& out_topic) { + const std::lock_guard lock(m_mutex); + + // Enforce that all relays have a unique input topic + if (m_relay_topics.find(in_topic) != m_relay_topics.end()) { + if (m_verbose) + printf("Already subscribed to relay from topic %s\n", in_topic.c_str()); + return false; + } + + // Enforce that all relays have a unique output topic + // The republishing side will already be checking for this but may have no way + // to communicate that to us, so we check too + std::map::iterator iter = m_relay_topics.begin(); + while (iter != m_relay_topics.end()) { + if (iter->second.out_topic == out_topic) { + if (m_verbose) + printf("Already relaying to topic %s\n", out_topic.c_str()); + return false; + } + iter++; + } + + RelayTopicInfo topic_info; + topic_info.out_topic = out_topic; + + topic_info.sub = rosSubscribe(in_topic); + // handleRelayedMessage() may immediately start getting called + + m_relay_topics[in_topic] = topic_info; + + // let implementation know + subscribeTopic(in_topic, topic_info); + + return true; +} diff --git a/communications/comms_bridge/src/dds_ros_bridge_publisher.cpp b/communications/comms_bridge/src/dds_ros_bridge_publisher.cpp new file mode 100644 index 0000000000..da1d525512 --- /dev/null +++ b/communications/comms_bridge/src/dds_ros_bridge_publisher.cpp @@ -0,0 +1,32 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "comms_bridge/dds_ros_bridge_publisher.h" + +DDSROSBridgePublisher::DDSROSBridgePublisher(double ad2pub_delay) : BridgePublisher(ad2pub_delay) {} + +DDSROSBridgePublisher::~DDSROSBridgePublisher() {} + +// When the peered subscriber transmits advertisement information, call: +// advertiseTopic(const std::string &output_topic, const AdvertisementInfo &ad_info) +// while holding m_mutex +// to setup the output ROS advertisement + +// When the peered subscriber transmits a serialized message, call: +// relayMessage(RelayTopicInfo &topic_info, const ContentInfo &content_info) +// to publish the output ROS message diff --git a/communications/comms_bridge/src/ros_dds_bridge_subscriber.cpp b/communications/comms_bridge/src/ros_dds_bridge_subscriber.cpp new file mode 100644 index 0000000000..ef3449ed52 --- /dev/null +++ b/communications/comms_bridge/src/ros_dds_bridge_subscriber.cpp @@ -0,0 +1,45 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + + +#include + +#include "comms_bridge/ros_dds_bridge_subscriber.h" + +ROSDDSBridgeSubscriber::ROSDDSBridgeSubscriber() { + // FIXME: any needed setup +} + +ROSDDSBridgeSubscriber::~ROSDDSBridgeSubscriber() { + // FIXME: any needed cleanup +} + +// Called with the mutex held +void ROSDDSBridgeSubscriber::subscribeTopic(std::string const& in_topic, const RelayTopicInfo& info) { + // FIXME: DDS stuff here +} + +// Called with the mutex held +void ROSDDSBridgeSubscriber::advertiseTopic(const RelayTopicInfo& info) { + // FIXME: DDS stuff here +} + +// Called with the mutex held +void ROSDDSBridgeSubscriber::relayMessage(const RelayTopicInfo& topic_info, ContentInfo const& content_info) { + // FIXME: DDS stuff here +} diff --git a/communications/comms_bridge/src/ros_ros_bridge_publisher.cpp b/communications/comms_bridge/src/ros_ros_bridge_publisher.cpp new file mode 100644 index 0000000000..0aa9d0ef32 --- /dev/null +++ b/communications/comms_bridge/src/ros_ros_bridge_publisher.cpp @@ -0,0 +1,121 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include +#include + +#include "comms_bridge/ros_ros_bridge_publisher.h" + +// ROS internal subscriber message queue size +#define SUBSCRIBER_QUEUE_SIZE 10 +// ROS internal publisher message queue size +#define PUBLISHER_QUEUE_SIZE 10 + +ROSROSBridgePublisher::ROSROSBridgePublisher(const std::string& meta_topic_prefix, double ad2pub_delay) + : ROSBridgePublisher(ad2pub_delay), m_meta_topic_prefix(meta_topic_prefix) { + setupMetaChannels(); + setupReverseMetaChannels(); +} + +ROSROSBridgePublisher::~ROSROSBridgePublisher() {} + +void ROSROSBridgePublisher::setupMetaChannels() { + ros::NodeHandle nh; + + std::string advertisement_topic = m_meta_topic_prefix + "/advert"; + if (m_verbose > 0) + printf("advert topic = %s\n", advertisement_topic.c_str()); + m_advert_sub = + nh.subscribe(advertisement_topic, SUBSCRIBER_QUEUE_SIZE, &ROSROSBridgePublisher::handleAdMessage, this); + + std::string content_topic = m_meta_topic_prefix + "/content"; + if (m_verbose > 0) + printf("content topic = %s\n", content_topic.c_str()); + m_content_sub = + nh.subscribe(content_topic, SUBSCRIBER_QUEUE_SIZE, &ROSROSBridgePublisher::handleContentMessage, this); +} + +void ROSROSBridgePublisher::setupReverseMetaChannels() { + ros::NodeHandle nh; + + std::string reset_topic = m_meta_topic_prefix + "/reset"; + m_reset_pub = nh.advertise(reset_topic, PUBLISHER_QUEUE_SIZE); +} + +void ROSROSBridgePublisher::requestTopicInfo(std::string const& output_topic) { + ros_bridge::RelayReset reset_msg; + + reset_msg.header.seq = 0; + reset_msg.header.stamp = ros::Time::now(); + + reset_msg.topic = output_topic; + + m_reset_pub.publish(reset_msg); +} + +void ROSROSBridgePublisher::handleContentMessage(const ros_bridge::RelayContent::ConstPtr& msg) { + const std::lock_guard lock(m_mutex); + + const std::string output_topic = msg->output_topic; + + if (m_verbose) + printf("Received content message for topic %s\n", output_topic.c_str()); + + std::map::iterator iter = m_relay_topics.find(output_topic); + if (iter == m_relay_topics.end()) { + // first time seeing this topic + // The advertisement that preceded this was either lost, delayed out of + // order, or we the publisher were restarted after it was sent + if (m_verbose) + printf(" first seeing topic\n"); + + RelayTopicInfo topic_info; + topic_info.out_topic = output_topic; + topic_info.ad_info.md5_sum = msg->type_md5_sum; + iter = m_relay_topics.emplace(output_topic, topic_info).first; + + requestTopicInfo(output_topic); + } + + RelayTopicInfo &topic_info = iter->second; + + ContentInfo content_info; + content_info.type_md5_sum = msg->type_md5_sum; + content_info.data = msg->msg; + + if (!relayMessage(topic_info, content_info)) + printf(" error relaying message\n"); +} + +void ROSROSBridgePublisher::handleAdMessage(const ros_bridge::RelayAdvertisement::ConstPtr& msg) { + const std::lock_guard lock(m_mutex); + + const std::string output_topic = msg->output_topic; + + if (m_verbose) + printf("Received advertisement message for topic %s\n", output_topic.c_str()); + + AdvertisementInfo ad_info; + ad_info.md5_sum = msg->md5_sum; + ad_info.data_type = msg->data_type; + ad_info.definition = msg->definition; + + if (!advertiseTopic(msg->output_topic, ad_info)) + printf(" error advertising topic\n"); +} diff --git a/communications/comms_bridge/src/ros_ros_bridge_subscriber.cpp b/communications/comms_bridge/src/ros_ros_bridge_subscriber.cpp new file mode 100644 index 0000000000..f7ed03ceaa --- /dev/null +++ b/communications/comms_bridge/src/ros_ros_bridge_subscriber.cpp @@ -0,0 +1,124 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include +#include + +#include "comms_bridge/ros_ros_bridge_subscriber.h" + +// ROS internal subscriber message queue size +#define SUBSCRIBER_QUEUE_SIZE 10 +// ROS internal publisher message queue size +#define PUBLISHER_QUEUE_SIZE 10 + +ROSROSBridgeSubscriber::ROSROSBridgeSubscriber(const std::string& meta_topic_prefix) + : m_meta_topic_prefix(meta_topic_prefix) { + m_n_advertised = 0; + + setupMetaChannels(); + setupReverseMetaChannels(); +} + +ROSROSBridgeSubscriber::~ROSROSBridgeSubscriber() {} + +void ROSROSBridgeSubscriber::setupMetaChannels() { + ros::NodeHandle nh; + + std::string advertisement_topic = m_meta_topic_prefix + "/advert"; + m_advertiser_pub = nh.advertise(advertisement_topic, PUBLISHER_QUEUE_SIZE); + + std::string content_topic = m_meta_topic_prefix + "/content"; + m_relayer_pub = nh.advertise(content_topic, PUBLISHER_QUEUE_SIZE); +} + +void ROSROSBridgeSubscriber::setupReverseMetaChannels() { + ros::NodeHandle nh; + + std::string reset_topic = m_meta_topic_prefix + "/reset"; + m_reset_sub = nh.subscribe(reset_topic, SUBSCRIBER_QUEUE_SIZE, &ROSROSBridgeSubscriber::handleResetMessage, this); +} + +void ROSROSBridgeSubscriber::handleResetMessage(const ros_bridge::RelayReset::ConstPtr& msg) { + const std::string out_topic = msg->topic; + + const std::lock_guard lock(m_mutex); + + std::map::iterator iter = m_relay_topics.begin(); + while (iter != m_relay_topics.end()) { + if (iter->second.out_topic == out_topic) + break; + iter++; + } + + if (iter == m_relay_topics.end()) { + if (m_verbose) + printf("Received reset on unknown topic %s\n", out_topic.c_str()); + return; + } + + if (m_verbose) + printf("Received reset for topic %s\n", out_topic.c_str()); + + requested_resets[out_topic] = true; +} + +void ROSROSBridgeSubscriber::subscribeTopic(std::string const& in_topic, const RelayTopicInfo& info) { + // this is just the base subscriber letting us know it's adding a topic + // nothing more we need to do +} + +void ROSROSBridgeSubscriber::advertiseTopic(const RelayTopicInfo& info) { + const AdvertisementInfo &ad_info = info.ad_info; + + ros_bridge::RelayAdvertisement ad; + + ad.header.seq = ++m_n_advertised; + ad.header.stamp = ros::Time::now(); + + ad.output_topic = info.out_topic; + + ad.latching = ad_info.latching; + + ad.data_type = ad_info.data_type; + ad.md5_sum = ad_info.md5_sum; + ad.definition = ad_info.definition; + + m_advertiser_pub.publish(ad); +} + +void ROSROSBridgeSubscriber::relayMessage(const RelayTopicInfo& topic_info, ContentInfo const& content_info) { + std::map::iterator iter = requested_resets.find(topic_info.out_topic); + if (iter != requested_resets.end()) { + advertiseTopic(topic_info); + requested_resets.erase(iter); + } + + ros_bridge::RelayContent content; + + content.header.seq = topic_info.relay_seqnum; + content.header.stamp = ros::Time::now(); + + content.output_topic = topic_info.out_topic; + + content.type_md5_sum = content_info.type_md5_sum; + + content.msg.resize(content_info.data_size); + std::memcpy(static_cast(content.msg.data()), static_cast(content_info.data), content.msg.size()); + + m_relayer_pub.publish(content); +} diff --git a/communications/ff_msgs/msg/RelayAdvertisement.msg b/communications/ff_msgs/msg/RelayAdvertisement.msg new file mode 100644 index 0000000000..f0d49d208b --- /dev/null +++ b/communications/ff_msgs/msg/RelayAdvertisement.msg @@ -0,0 +1,17 @@ +# timestamp in header should be time of whatever triggered the advertisement +std_msgs/Header header + +#topic on which to republish +string output_topic + +#whether should be republished as a latching topic +bool latching + +#standard ROS meanings for message descriptions +# eg std_msgs/Int32 +string data_type +# eg da5909fbe378aeaf85e547e830cc1bb7 +string md5_sum +# eg a definition such as this file +string definition + diff --git a/communications/ff_msgs/msg/RelayContent.msg b/communications/ff_msgs/msg/RelayContent.msg new file mode 100644 index 0000000000..c768922328 --- /dev/null +++ b/communications/ff_msgs/msg/RelayContent.msg @@ -0,0 +1,13 @@ +# timestamp in header should be time of receipt of original message +std_msgs/Header header + +#topic on which to republish +string output_topic + +#md5sum of msg type, repeated from advertisement +string type_md5_sum + +#serialized content of the message +#definition, etc. assumed to be known from preceding advertisement +uint8[] msg + diff --git a/communications/ff_msgs/msg/RelayReset.msg b/communications/ff_msgs/msg/RelayReset.msg new file mode 100644 index 0000000000..397561bffd --- /dev/null +++ b/communications/ff_msgs/msg/RelayReset.msg @@ -0,0 +1,7 @@ +# timestamp in header should be time of whatever triggered the reset +std_msgs/Header header + +#topic for which metadata retransmission is requested +string topic + + diff --git a/communications/readme.md b/communications/readme.md index 81177cbefd..970753285b 100644 --- a/communications/readme.md +++ b/communications/readme.md @@ -1,3 +1,4 @@ \page comms Communications \subpage dds_ros_bridge +\subpage comms_bridge diff --git a/scripts/git/cpplint.py b/scripts/git/cpplint.py index 300489ed0c..f5ff8045cd 100755 --- a/scripts/git/cpplint.py +++ b/scripts/git/cpplint.py @@ -6660,8 +6660,8 @@ def FlagCxx11Features(filename, clean_lines, linenum, error): if include and include.group(1) in ( "cfenv", "fenv.h", - "condition_variable", - "future", + # "condition_variable", + # "future", "ratio", "regex", "system_error", From 2fdfff5cf3087586049060f63690d8448264c951 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Tue, 31 Oct 2023 14:46:01 -0700 Subject: [PATCH 02/32] skip submodules in linter --- scripts/git/pre-commit.linter_python | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/scripts/git/pre-commit.linter_python b/scripts/git/pre-commit.linter_python index db90dac612..01b8f7f5bb 100755 --- a/scripts/git/pre-commit.linter_python +++ b/scripts/git/pre-commit.linter_python @@ -71,11 +71,11 @@ echo " Analysing python code style with 'isort'." # could happen for example if the .isort.cfg src_paths list gets out # of date. -if $(isort . --extend-skip cmake --profile black --diff --check-only --quiet >/dev/null); then +if $(isort . --extend-skip cmake,submodules --profile black --diff --check-only --quiet >/dev/null); then echo "Linter checks using 'isort' passed." else echo "Errors detected with 'isort'. Fixing them. Try to add and commit your files again." - isort . --extend-skip cmake --profile black >/dev/null + isort . --extend-skip cmake,submodules --profile black >/dev/null failed_lint=true fi From d8f37733daecd5aff9cfbc7a5ccd8f960ce82804 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Tue, 31 Oct 2023 15:20:28 -0700 Subject: [PATCH 03/32] fixing ros ros compile --- .../include/comms_bridge/ros_ros_bridge_publisher.h | 8 ++++---- .../include/comms_bridge/ros_ros_bridge_subscriber.h | 4 ++-- .../comms_bridge/src/ros_ros_bridge_publisher.cpp | 10 +++++----- .../comms_bridge/src/ros_ros_bridge_subscriber.cpp | 10 +++++----- 4 files changed, 16 insertions(+), 16 deletions(-) diff --git a/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_publisher.h b/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_publisher.h index 7bc0f308b9..be1e5bca6a 100644 --- a/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_publisher.h +++ b/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_publisher.h @@ -36,7 +36,7 @@ // default time to delay between advertising and publishing on that topic [sec] #define DEFAULT_ROSROSBRIDGE_PUB_ADVERTISE_DELAY 3.0 -class ROSROSBridgePublisher : public ROSBridgePublisher { +class ROSROSBridgePublisher : public BridgePublisher { public: ROSROSBridgePublisher(const std::string& meta_topic_prefix = DEFAULT_ROSROSBRIDGE_PUB_META_TOPIC_PREFIX, double ad2pub_delay = DEFAULT_ROSROSBRIDGE_PUB_ADVERTISE_DELAY); @@ -48,8 +48,8 @@ class ROSROSBridgePublisher : public ROSBridgePublisher { void requestTopicInfo(std::string const& output_topic); - void handleContentMessage(const ros_bridge::RelayContent::ConstPtr& msg); - void handleAdMessage(const ros_bridge::RelayAdvertisement::ConstPtr& msg); + void handleContentMessage(const ff_msgs::RelayContent::ConstPtr& msg); + void handleAdMessage(const ff_msgs::RelayAdvertisement::ConstPtr& msg); std::string m_meta_topic_prefix; ros::Subscriber m_advert_sub; @@ -57,7 +57,7 @@ class ROSROSBridgePublisher : public ROSBridgePublisher { ros::Publisher m_reset_pub; // prohibit shallow copy or assignment - ROSROSBridgePublisher(const ROSROSBridgePublisher&) : ROSBridgePublisher(m_ad2pub_delay) {} + ROSROSBridgePublisher(const ROSROSBridgePublisher&) : BridgePublisher(m_ad2pub_delay) {} void operator=(const ROSROSBridgePublisher&) {} }; diff --git a/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_subscriber.h b/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_subscriber.h index 854da6d709..c53650d2be 100644 --- a/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_subscriber.h +++ b/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_subscriber.h @@ -35,7 +35,7 @@ #define DEFAULT_ROSROSBRIDGE_PUB_META_TOPIC_PREFIX "/polymorph_relay" -class ROSROSBridgeSubscriber : public ROSBridgeSubscriber { +class ROSROSBridgeSubscriber : public BridgeSubscriber { public: explicit ROSROSBridgeSubscriber(const std::string& meta_topic_prefix = DEFAULT_ROSROSBRIDGE_PUB_META_TOPIC_PREFIX); virtual ~ROSROSBridgeSubscriber(); @@ -52,7 +52,7 @@ class ROSROSBridgeSubscriber : public ROSBridgeSubscriber { protected: void setupMetaChannels(); void setupReverseMetaChannels(); - void handleResetMessage(const ros_bridge::RelayReset::ConstPtr& msg); + void handleResetMessage(const ff_msgs::RelayReset::ConstPtr& msg); std::string m_meta_topic_prefix; unsigned int m_n_advertised; diff --git a/communications/comms_bridge/src/ros_ros_bridge_publisher.cpp b/communications/comms_bridge/src/ros_ros_bridge_publisher.cpp index 0aa9d0ef32..40c018622c 100644 --- a/communications/comms_bridge/src/ros_ros_bridge_publisher.cpp +++ b/communications/comms_bridge/src/ros_ros_bridge_publisher.cpp @@ -28,7 +28,7 @@ #define PUBLISHER_QUEUE_SIZE 10 ROSROSBridgePublisher::ROSROSBridgePublisher(const std::string& meta_topic_prefix, double ad2pub_delay) - : ROSBridgePublisher(ad2pub_delay), m_meta_topic_prefix(meta_topic_prefix) { + : BridgePublisher(ad2pub_delay), m_meta_topic_prefix(meta_topic_prefix) { setupMetaChannels(); setupReverseMetaChannels(); } @@ -55,11 +55,11 @@ void ROSROSBridgePublisher::setupReverseMetaChannels() { ros::NodeHandle nh; std::string reset_topic = m_meta_topic_prefix + "/reset"; - m_reset_pub = nh.advertise(reset_topic, PUBLISHER_QUEUE_SIZE); + m_reset_pub = nh.advertise(reset_topic, PUBLISHER_QUEUE_SIZE); } void ROSROSBridgePublisher::requestTopicInfo(std::string const& output_topic) { - ros_bridge::RelayReset reset_msg; + ff_msgs::RelayReset reset_msg; reset_msg.header.seq = 0; reset_msg.header.stamp = ros::Time::now(); @@ -69,7 +69,7 @@ void ROSROSBridgePublisher::requestTopicInfo(std::string const& output_topic) { m_reset_pub.publish(reset_msg); } -void ROSROSBridgePublisher::handleContentMessage(const ros_bridge::RelayContent::ConstPtr& msg) { +void ROSROSBridgePublisher::handleContentMessage(const ff_msgs::RelayContent::ConstPtr& msg) { const std::lock_guard lock(m_mutex); const std::string output_topic = msg->output_topic; @@ -103,7 +103,7 @@ void ROSROSBridgePublisher::handleContentMessage(const ros_bridge::RelayContent: printf(" error relaying message\n"); } -void ROSROSBridgePublisher::handleAdMessage(const ros_bridge::RelayAdvertisement::ConstPtr& msg) { +void ROSROSBridgePublisher::handleAdMessage(const ff_msgs::RelayAdvertisement::ConstPtr& msg) { const std::lock_guard lock(m_mutex); const std::string output_topic = msg->output_topic; diff --git a/communications/comms_bridge/src/ros_ros_bridge_subscriber.cpp b/communications/comms_bridge/src/ros_ros_bridge_subscriber.cpp index f7ed03ceaa..ef53a7e5b9 100644 --- a/communications/comms_bridge/src/ros_ros_bridge_subscriber.cpp +++ b/communications/comms_bridge/src/ros_ros_bridge_subscriber.cpp @@ -40,10 +40,10 @@ void ROSROSBridgeSubscriber::setupMetaChannels() { ros::NodeHandle nh; std::string advertisement_topic = m_meta_topic_prefix + "/advert"; - m_advertiser_pub = nh.advertise(advertisement_topic, PUBLISHER_QUEUE_SIZE); + m_advertiser_pub = nh.advertise(advertisement_topic, PUBLISHER_QUEUE_SIZE); std::string content_topic = m_meta_topic_prefix + "/content"; - m_relayer_pub = nh.advertise(content_topic, PUBLISHER_QUEUE_SIZE); + m_relayer_pub = nh.advertise(content_topic, PUBLISHER_QUEUE_SIZE); } void ROSROSBridgeSubscriber::setupReverseMetaChannels() { @@ -53,7 +53,7 @@ void ROSROSBridgeSubscriber::setupReverseMetaChannels() { m_reset_sub = nh.subscribe(reset_topic, SUBSCRIBER_QUEUE_SIZE, &ROSROSBridgeSubscriber::handleResetMessage, this); } -void ROSROSBridgeSubscriber::handleResetMessage(const ros_bridge::RelayReset::ConstPtr& msg) { +void ROSROSBridgeSubscriber::handleResetMessage(const ff_msgs::RelayReset::ConstPtr& msg) { const std::string out_topic = msg->topic; const std::lock_guard lock(m_mutex); @@ -85,7 +85,7 @@ void ROSROSBridgeSubscriber::subscribeTopic(std::string const& in_topic, const R void ROSROSBridgeSubscriber::advertiseTopic(const RelayTopicInfo& info) { const AdvertisementInfo &ad_info = info.ad_info; - ros_bridge::RelayAdvertisement ad; + ff_msgs::RelayAdvertisement ad; ad.header.seq = ++m_n_advertised; ad.header.stamp = ros::Time::now(); @@ -108,7 +108,7 @@ void ROSROSBridgeSubscriber::relayMessage(const RelayTopicInfo& topic_info, Cont requested_resets.erase(iter); } - ros_bridge::RelayContent content; + ff_msgs::RelayContent content; content.header.seq = topic_info.relay_seqnum; content.header.stamp = ros::Time::now(); From dd2cef72f50c796e29e6ce5fde3ec743f93263ed Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Wed, 1 Nov 2023 11:40:13 -0700 Subject: [PATCH 04/32] adding launchfile stuff --- astrobee/launch/robot/MLP.launch | 20 ++++++++++++ .../comms_bridge/launch/comms_bridge.launch | 32 +++++++++++++++++++ .../comms_bridge/nodelet_plugins.xml | 12 +++---- communications/comms_bridge/package.xml | 6 +++- 4 files changed, 63 insertions(+), 7 deletions(-) create mode 100644 communications/comms_bridge/launch/comms_bridge.launch diff --git a/astrobee/launch/robot/MLP.launch b/astrobee/launch/robot/MLP.launch index 096cafee64..0eda37a6fc 100644 --- a/astrobee/launch/robot/MLP.launch +++ b/astrobee/launch/robot/MLP.launch @@ -296,6 +296,26 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/communications/comms_bridge/launch/comms_bridge.launch b/communications/comms_bridge/launch/comms_bridge.launch new file mode 100644 index 0000000000..8023e8ae6a --- /dev/null +++ b/communications/comms_bridge/launch/comms_bridge.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/communications/comms_bridge/nodelet_plugins.xml b/communications/comms_bridge/nodelet_plugins.xml index b7729ef427..693aea970d 100644 --- a/communications/comms_bridge/nodelet_plugins.xml +++ b/communications/comms_bridge/nodelet_plugins.xml @@ -1,13 +1,13 @@ - - + - Nodelet for the polymorph relay subscriber + Nodelet for the comms bridge subscriber - - - Nodelet for the polymorph relay publisher + + + Nodelet for the comms bridge publisher \ No newline at end of file diff --git a/communications/comms_bridge/package.xml b/communications/comms_bridge/package.xml index aff3ef03bc..11097a16b5 100644 --- a/communications/comms_bridge/package.xml +++ b/communications/comms_bridge/package.xml @@ -22,6 +22,7 @@ image_transport std_msgs sensor_msgs + ff_util ff_msgs topic_tools roscpp @@ -31,7 +32,10 @@ image_transport std_msgs sensor_msgs + ff_util ff_msgs topic_tools - + + + From c486ba683461224746c9ab36826b52f70b19139b Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Wed, 1 Nov 2023 13:57:30 -0700 Subject: [PATCH 05/32] adding nodelets + config file + slapped in some dds init to see if that location would work --- .../config/communications/comms_bridge.config | 73 ++++++++ communications/comms_bridge/CMakeLists.txt | 50 ++++- .../comms_bridge/ros_dds_bridge_subscriber.h | 14 +- .../src/bridge_publisher_nodelet.cpp | 110 +++++++++++ .../src/bridge_subscriber_nodelet.cpp | 172 ++++++++++++++++++ .../src/ros_dds_bridge_subscriber.cpp | 72 +++++++- .../src/astrobee_astrobee_bridge.cc | 2 +- 7 files changed, 486 insertions(+), 7 deletions(-) create mode 100644 astrobee/config/communications/comms_bridge.config create mode 100644 communications/comms_bridge/src/bridge_publisher_nodelet.cpp create mode 100644 communications/comms_bridge/src/bridge_subscriber_nodelet.cpp diff --git a/astrobee/config/communications/comms_bridge.config b/astrobee/config/communications/comms_bridge.config new file mode 100644 index 0000000000..8022c464bd --- /dev/null +++ b/astrobee/config/communications/comms_bridge.config @@ -0,0 +1,73 @@ +-- Copyright (c) 2017, United States Government, as represented by the +-- Administrator of the National Aeronautics and Space Administration. +-- +-- All rights reserved. +-- +-- The Astrobee platform is licensed under the Apache License, Version 2.0 +-- (the "License"); you may not use this file except in compliance with the +-- License. You may obtain a copy of the License at +-- +-- http://www.apache.org/licenses/LICENSE-2.0 +-- +-- Unless required by applicable law or agreed to in writing, software +-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +-- License for the specific language governing permissions and limitations +-- under the License. + +-- param for each bridge component use the following params -- +-- ABREV = First Letters of Component Ex: ros_string_rapid_text_msg = RSRTM -- +-- use_ABREV bool: turn this component on? -- +-- sub_topic_ABREV string: [ROS: subscribe topic] or [RAPID: subscribe topic suffix] -- +-- pub_topic_ABREV string: [ROS: publish topic] or [RAPID: publish topic suffix] -- +-- queue_size_ABREV int: queue size -- + +require "context" + +-- Enable DDS communications on start -- +-- If false, a trigger call will be needed to start communications -- +started = false; + +-- Rapid subscribers -- +rapid_sub_names = {"Bumble", "Honey", "Queen", "Bsharp", "Wannabee"} + +links = { + -- Logically, there could be up to three two-way links between the three robots. In practice, we + -- will probably only have one link. + { + -- A single link entry has required fields "from" and "to" that specify the robot roles involved + -- in the link. + from = {"Bumble"}, -- manager + to = {"Queen"}, -- actor + + -- The link entry has three optional fields: relay_forward (messages to be relayed only in the + -- `from` to `to` direction), relay_backward (to be relayed only in the `to` to `from` direction), + -- and relay_both (to be relayed in both directions). Providing all three fields gives the user + -- full directional control while minimizing repetition and copy/paste errors. + relay_forward = { + {name = "beh/inspection/goal"}, + {name = "beh/dock/goal"}, + {name = "gnc/control/goal"}, + }, + relay_backward = { + {name = "beh/inspection/result"}, + {name = "beh/dock/result"}, + {name = "gnc/control/result"}, + }, + relay_both = { + {name = "beh/inspection/cancel"}, + {name = "beh/inspection/feedback"}, + {name = "beh/inspection/state"}, + {name = "beh/inspection/status"}, + {name = "beh/dock/cancel"}, + {name = "beh/dock/feedback"}, + {name = "beh/dock/state"}, + {name = "beh/dock/status"}, + {name = "gnc/control/cancel"}, + {name = "gnc/control/feedback"}, + {name = "gnc/control/status"}, + } + } +} + +verbose = false \ No newline at end of file diff --git a/communications/comms_bridge/CMakeLists.txt b/communications/comms_bridge/CMakeLists.txt index 6674c2479e..7287acda24 100644 --- a/communications/comms_bridge/CMakeLists.txt +++ b/communications/comms_bridge/CMakeLists.txt @@ -31,10 +31,43 @@ find_package(catkin2 REQUIRED COMPONENTS image_transport std_msgs sensor_msgs + ff_util ff_msgs topic_tools ) + +# System dependencies are found with CMake's conventions +# find_package(Eigen3 REQUIRED) +find_package(Boost 1.54.0 QUIET REQUIRED COMPONENTS filesystem system iostreams thread program_options timer) + +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/../../cmake") + +if (USE_CTC) + set(SORACORE_ROOT_DIR ${ARM_CHROOT_DIR}/usr) +else (USE_CTC) + set(SORACORE_ROOT_DIR /usr) +endif (USE_CTC) + +set(MIRO_ROOT_DIR ${SORACORE_ROOT_DIR}) + +SET(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) +SET(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) + +# find Qt version according to OS +find_program(LSB_RELEASE_EXEC lsb_release) +execute_process(COMMAND "${LSB_RELEASE_EXEC}" --short --release OUTPUT_VARIABLE LSB_RELEASE_VERSION_SHORT OUTPUT_STRIP_TRAILING_WHITESPACE) + +if (${LSB_RELEASE_VERSION_SHORT} VERSION_GREATER 18.04) + find_package(Qt5Xml REQUIRED) +else () + find_package(Qt4 4.6.0 REQUIRED QtXml) +endif () + +find_package(Miro REQUIRED) +find_package(RtiDds REQUIRED) +find_package(Soracore REQUIRED) + catkin_package( LIBRARIES src CATKIN_DEPENDS @@ -50,6 +83,12 @@ catkin_package( include_directories( include ${catkin_INCLUDE_DIRS} + ${RTIDDS_INCLUDE_DIR} + ${SORACORE_INCLUDE_DIRS} + ${MIRO_INCLUDE_DIR} + ${QT_INCLUDE_DIR} + ${QT_INCLUDE_DIR}/Qt + ${Boost_INCLUDE_DIRS} ) @@ -57,19 +96,21 @@ include_directories( add_library(comms_bridge_sub src/bridge_subscriber.cpp src/ros_dds_bridge_subscriber.cpp + src/bridge_subscriber_nodelet.cpp ) add_dependencies(comms_bridge_sub ${catkin_EXPORTED_TARGETS}) target_link_libraries(comms_bridge_sub - ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${CMAKE_DL_LIBS} ) + rapidIo Qt5::Xml ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${CMAKE_DL_LIBS} ) add_library(comms_bridge_pub src/bridge_publisher.cpp src/dds_ros_bridge_publisher.cpp + src/bridge_publisher_nodelet.cpp ) add_dependencies(comms_bridge_pub ${catkin_EXPORTED_TARGETS} ) target_link_libraries(comms_bridge_pub - ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${CMAKE_DL_LIBS} ) + rapidIo Qt5::Xml ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${CMAKE_DL_LIBS} ) ############# @@ -86,3 +127,8 @@ install(TARGETS ${PROJECT_NAME}_sub ${PROJECT_NAME}_pub install(FILES nodelet_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) + +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) \ No newline at end of file diff --git a/communications/comms_bridge/include/comms_bridge/ros_dds_bridge_subscriber.h b/communications/comms_bridge/include/comms_bridge/ros_dds_bridge_subscriber.h index 60dccc72b6..83fa348490 100644 --- a/communications/comms_bridge/include/comms_bridge/ros_dds_bridge_subscriber.h +++ b/communications/comms_bridge/include/comms_bridge/ros_dds_bridge_subscriber.h @@ -28,11 +28,21 @@ #include +// // SoraCore Includes +// #include "knDds/DdsSupport.h" +// #include "knDds/DdsEntitiesFactory.h" +// #include "knDds/DdsEntitiesFactorySvc.h" + +// miro includes +#include +#include +#include + #include class ROSDDSBridgeSubscriber : public BridgeSubscriber { public: - ROSDDSBridgeSubscriber(); + explicit ROSDDSBridgeSubscriber(std::string agent_name); virtual ~ROSDDSBridgeSubscriber(); // Called with the mutex held @@ -48,6 +58,8 @@ class ROSDDSBridgeSubscriber : public BridgeSubscriber { // prohibit shallow copy or assignment ROSDDSBridgeSubscriber(const ROSDDSBridgeSubscriber&) {} void operator=(const ROSDDSBridgeSubscriber&) {} + private: + std::string agent_name_, participant_name_; }; #endif // COMMS_BRIDGE_ROS_DDS_BRIDGE_SUBSCRIBER_H_ diff --git a/communications/comms_bridge/src/bridge_publisher_nodelet.cpp b/communications/comms_bridge/src/bridge_publisher_nodelet.cpp new file mode 100644 index 0000000000..d2d8b615f5 --- /dev/null +++ b/communications/comms_bridge/src/bridge_publisher_nodelet.cpp @@ -0,0 +1,110 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +// Standard ROS includes +#include +#include +#include + +// FSW shared libraries +#include + +// FSW nodelet +#include +#include + +#include +#include + +#include "comms_bridge/dds_ros_bridge_publisher.h" + + +namespace comms_bridge { + +// void usage(const char *progname) +// { +// fprintf(stderr, "Usage: %s [-v] [-a] [-d ] [-t meta_topic_prefix]\n", +// progname); +// } + +class BridgePublisherNodelet : public ff_util::FreeFlyerNodelet { + public: + BridgePublisherNodelet() : ff_util::FreeFlyerNodelet("comms_bridge_pub") {} + + virtual ~BridgePublisherNodelet() {} + + protected: + virtual void Initialize(ros::NodeHandle* nh) { + // bool anonymous_node = false; + // unsigned int verbose = 0; + // std::string meta_topic_prefix = "/polymorph_relay"; + // double ad2pub_delay = 3.0; + + // int c; + // while ((c = getopt(argc, argv, "avd:t:")) != -1) { + // switch (c) { + // case 'a': + // anonymous_node = true; + // break; + // case 'v': + // verbose++; + // break; + // case 'd': + // ad2pub_delay = atof(optarg); + // if (ad2pub_delay < 0 || std::isnan(ad2pub_delay)) { + // fprintf(stderr, "Invalid advertise delay %s\n", optarg); + // usage(argv[0]); + // return 1; + // } + // break; + // case 't': + // meta_topic_prefix = optarg; + // break; + // case '?': + // default: + // usage(argv[0]); + // return 1; + // break; + // } + // } + + // if (argc-optind != 0) { + // usage(argv[0]); + // return 1; + // } + + // ros::init(argc, argv, "polymorph_relay_pub", (anonymous_node ? ros::init_options::AnonymousName : 0)); + // ros::NodeHandle nhp("~"); + // //nhp.param("verbose", verbose, 0); + + // ROSROSBridgePublisher pub(meta_topic_prefix, ad2pub_delay); + // if (verbose > 0) + // pub.setVerbosity(verbose); + + // printf("Waiting for messages\n"); + // ros::spin(); + + // printf("Exiting\n"); + + // return 0; + } +}; + +PLUGINLIB_EXPORT_CLASS(comms_bridge::BridgePublisherNodelet, nodelet::Nodelet) + +} // namespace comms_bridge diff --git a/communications/comms_bridge/src/bridge_subscriber_nodelet.cpp b/communications/comms_bridge/src/bridge_subscriber_nodelet.cpp new file mode 100644 index 0000000000..ba5e8dc222 --- /dev/null +++ b/communications/comms_bridge/src/bridge_subscriber_nodelet.cpp @@ -0,0 +1,172 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +// Standard ROS includes +#include +#include +#include + +// FSW shared libraries +#include + +// FSW nodelet +#include +#include + +#include +#include + +#include + +#include "comms_bridge/ros_dds_bridge_subscriber.h" + +namespace comms_bridge { +// void usage(const char *progname) +// { +// fprintf(stderr, "Usage: %s [-v] [-a] [-t meta_topic_prefix] [ ...]\n", progname); +// } + +class BridgeSubscriberNodelet : public ff_util::FreeFlyerNodelet { + public: + BridgeSubscriberNodelet() : ff_util::FreeFlyerNodelet("comms_bridge_sub") {} + + virtual ~BridgeSubscriberNodelet() {} + + protected: + virtual void Initialize(ros::NodeHandle* nh) { + // Need to get robot name which is in the lua config files, add files, read + // files, and get robot name but don't get the rest of the config parameters + // since these params are used to create the classes that use rapid dds. Need + // to set up Miro/DDs before reading the parameters. + + config_params_.AddFile("communications/comms_bridge.config"); + + if (!config_params_.ReadFiles()) { + ROS_FATAL("BridgeSubscriberNodelet: Error reading config files."); + exit(EXIT_FAILURE); + return; + } + + if (!config_params_.GetStr("agent_name", &agent_name_)) { + ROS_FATAL("BridgeSubscriberNodelet: Could not read robot name."); + exit(EXIT_FAILURE); + return; + } + + // In simulation, the namespace is usually set to the robot name so we need to + // check if we are in simulation and get the right name + if (agent_name_ == "sim" || agent_name_ == "simulator") { + // The platform name should be the simulated robot name + agent_name_ = GetPlatform(); + + // If there is not robot name, set it to a default name so that we can + // connect to the bridge + if (agent_name_ == "") { + agent_name_ = "Bumble"; + } else { + // Make sure that first letter of robot name is capitialized. GDS only + // recognizes capitialized robot names. + agent_name_[0] = toupper(agent_name_[0]); + } + } + + bool verbose; + if (!config_params_.GetBool("verbose", &verbose)) { + ROS_FATAL("BridgeSubscriberNodelet: Could not read verbosity level."); + exit(EXIT_FAILURE); + return; + } + + ROS_ERROR_STREAM("ROBOT NAME " << agent_name_); + // Declare the ROS to DDS Subscriber class + ROSDDSBridgeSubscriber sub(agent_name_); + std::string ns = "/" + std::tolower(agent_name_[0]) + agent_name_.substr(1) + "/"; + + // Load shared topic groups + config_reader::ConfigReader::Table links, link, relay_forward, relay_backward, relay_both, item_conf; + if (!config_params_.GetTable("links", &links)) { + ROS_FATAL("BridgeSubscriberNodelet: Links not specified!"); + return; + } + + // Need to search for the collision distance in the mapper parameters + for (int i = 1; i <= links.GetSize(); i++) { + if (!links.GetTable(i, &link)) { + NODELET_ERROR("Could not read the mapper parameter table row %i", i); + continue; + } + std::string config_agent, topic_name; + if (link.GetStr("from", &config_agent) && config_agent == agent_name_) { + if (link.GetTable("relay_forward", &relay_forward)) { + for (int j = 1; j <= relay_forward.GetSize(); j++) { + // Agent topic configuration + relay_forward.GetTable(j, &item_conf); + if (!item_conf.GetStr("name", &topic_name)) { + NODELET_ERROR("BridgeSubscriberNodelet: agent topic name not specified!"); + return; + } + assert(sub.addTopic(topic_name, ns + topic_name)); + } + } + if (link.GetTable("relay_both", &relay_both)) { + for (int j = 1; j <= relay_both.GetSize(); j++) { + // Agent topic configuration + relay_both.GetTable(j, &item_conf); + if (!item_conf.GetStr("name", &topic_name)) { + NODELET_ERROR("BridgeSubscriberNodelet: agent topic name not specified!"); + return; + } + assert(sub.addTopic(topic_name, ns + topic_name)); + } + } + } else if (link.GetStr("to", &config_agent) && config_agent == agent_name_) { + if (link.GetTable("relay_backward", &relay_backward)) { + for (int j = 1; j <= relay_backward.GetSize(); j++) { + // Agent topic configuration + relay_backward.GetTable(j, &item_conf); + if (!item_conf.GetStr("name", &topic_name)) { + NODELET_ERROR("BridgeSubscriberNodelet: agent topic name not specified!"); + return; + } + assert(sub.addTopic(topic_name, ns + topic_name)); + } + } + if (link.GetTable("relay_both", &relay_both)) { + for (int j = 1; j <= relay_both.GetSize(); j++) { + // Agent topic configuration + relay_both.GetTable(j, &item_conf); + if (!item_conf.GetStr("name", &topic_name)) { + NODELET_ERROR("BridgeSubscriberNodelet: agent topic name not specified!"); + return; + } + assert(sub.addTopic(topic_name, ns + topic_name)); + } + } + } + } + } + + private: + config_reader::ConfigReader config_params_; + std::string agent_name_; +}; + +PLUGINLIB_EXPORT_CLASS(comms_bridge::BridgeSubscriberNodelet, nodelet::Nodelet) + +} // namespace comms_bridge diff --git a/communications/comms_bridge/src/ros_dds_bridge_subscriber.cpp b/communications/comms_bridge/src/ros_dds_bridge_subscriber.cpp index ef3449ed52..874c7e6b6a 100644 --- a/communications/comms_bridge/src/ros_dds_bridge_subscriber.cpp +++ b/communications/comms_bridge/src/ros_dds_bridge_subscriber.cpp @@ -16,13 +16,79 @@ * under the License. */ +#include "comms_bridge/ros_dds_bridge_subscriber.h" #include -#include "comms_bridge/ros_dds_bridge_subscriber.h" +ROSDDSBridgeSubscriber::ROSDDSBridgeSubscriber(std::string agent_name) { + agent_name_ = agent_name; + + + int fake_argc = 1; + // Create fake argv containing only the participant name + // Participant name needs to uniue so combine robot name with timestamp + ros::Time time = ros::Time::now(); + participant_name_ = agent_name_ + std::to_string(time.sec) + + std::string("-multi-bridge"); + char **fake_argv = new char*[1]; + fake_argv[0] = new char[(participant_name_.size() + 1)]; + std::strcpy(fake_argv[0], participant_name_.c_str()); // NOLINT + + /* fake miro log into thinking we have no arguments */ + Miro::Log::init(fake_argc, fake_argv); + Miro::Log::level(9); + + /* fake miro configuration into thinking we have no arguments */ + Miro::Configuration::init(fake_argc, fake_argv); + + // Miro::RobotParameters *robot_params = Miro::RobotParameters::instance(); + // kn::DdsEntitiesFactorySvcParameters *dds_params = + // kn::DdsEntitiesFactorySvcParameters::instance(); + + // /* get the defaults for *all the things!* */ + // Miro::ConfigDocument *config = Miro::Configuration::document(); + // config->setSection("Robot"); + // config->getParameters("Miro::RobotParameters", *robot_params); + // config->getParameters("kn::DdsEntitiesFactorySvcParameters", *dds_params); + + // robot_params->name = agent_name_; + // robot_params->namingContextName = robot_params->name; + + // // Set values for default publisher and susbcriber + // dds_params->publishers[0].name = agent_name_; + // dds_params->publishers[0].partition = agent_name_; + // dds_params->publishers[0].participant = participant_name_; + // dds_params->subscribers[0].participant = participant_name_; + + // // Clear config files so that dds only looks for the files we add + // dds_params->participants[0].discoveryPeersFiles.clear(); + // dds_params->configFiles.clear(); + + // dds_params->participants[0].name = participant_name_; + // dds_params->participants[0].participantName = participant_name_; + // dds_params->participants[0].domainId = 38; + // dds_params->participants[0].discoveryPeersFiles.push_back( + // (config_path + "NDDS_DISCOVERY_PEERS")); + // dds_params->configFiles.push_back((config_path + "RAPID_QOS_PROFILES.xml")); + + + // /** + // * Use DdsEntitiesFactorySvc to create a new DdsEntitiesFactory + // * which will create all objects: + // * Participants DdsDomainParticpantRepository::instance() + // * Publishers DdsPublisherRespoitory::instance() + // * Subscribers DdsSubscriberRepository::instance() + // * Topics + // * and store in relevant repository + // * based on DdsEntitesFactoryParameters + // */ + // dds_entities_factory_.reset(new kn::DdsEntitiesFactorySvc()); + // dds_entities_factory_->init(dds_params); -ROSDDSBridgeSubscriber::ROSDDSBridgeSubscriber() { - // FIXME: any needed setup + // trigger_srv_ = nh->advertiseService( + // SERVICE_COMMUNICATIONS_ENABLE_BRIDGE_SUBSCRIBER, + // &AstrobeeAstrobeeBridge::Start, + // this); } ROSDDSBridgeSubscriber::~ROSDDSBridgeSubscriber() { diff --git a/communications/dds_ros_bridge/src/astrobee_astrobee_bridge.cc b/communications/dds_ros_bridge/src/astrobee_astrobee_bridge.cc index 9cbfd3b547..ee87fe924b 100644 --- a/communications/dds_ros_bridge/src/astrobee_astrobee_bridge.cc +++ b/communications/dds_ros_bridge/src/astrobee_astrobee_bridge.cc @@ -83,7 +83,7 @@ void AstrobeeAstrobeeBridge::Initialize(ros::NodeHandle *nh) { std::string config_path = ff_common::GetConfigDir(); config_path += "/communications/dds_intercomms/"; - // Create fake argv containing only the particaptant name + // Create fake argv containing only the participant name // Participant name needs to uniue so combine robot name with timestamp ros::Time time = ros::Time::now(); participant_name_ = agent_name_ + std::to_string(time.sec) From dbb9e019f73386678bcffed32392ad941eadf0a1 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Wed, 1 Nov 2023 14:34:04 -0700 Subject: [PATCH 06/32] not compiling unless dds is on --- communications/comms_bridge/CMakeLists.txt | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/communications/comms_bridge/CMakeLists.txt b/communications/comms_bridge/CMakeLists.txt index 7287acda24..6af17b55b6 100644 --- a/communications/comms_bridge/CMakeLists.txt +++ b/communications/comms_bridge/CMakeLists.txt @@ -22,6 +22,7 @@ project(comms_bridge) ## Compile as C++14, supported in ROS Kinetic and newer add_compile_options(-std=c++14) +if (USE_DDS) ## Find catkin macros and libraries find_package(catkin2 REQUIRED COMPONENTS roscpp @@ -131,4 +132,9 @@ install(FILES nodelet_plugins.xml # Mark launch files for installation install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch - PATTERN ".svn" EXCLUDE) \ No newline at end of file + PATTERN ".svn" EXCLUDE) + +else (USE_DDS) + find_package(catkin REQUIRED COMPONENTS) + catkin_package() +endif (USE_DDS) \ No newline at end of file From 81008e0c892effd9c4141a0f6a1904ff49b18031 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Wed, 1 Nov 2023 15:32:06 -0700 Subject: [PATCH 07/32] making sure topics are being passed in + fixing things --- .../config/communications/comms_bridge.config | 6 ++--- .../comms_bridge/src/bridge_subscriber.cpp | 22 ++++++++--------- .../src/bridge_subscriber_nodelet.cpp | 24 +++++++++---------- 3 files changed, 25 insertions(+), 27 deletions(-) diff --git a/astrobee/config/communications/comms_bridge.config b/astrobee/config/communications/comms_bridge.config index 8022c464bd..4413ffff3e 100644 --- a/astrobee/config/communications/comms_bridge.config +++ b/astrobee/config/communications/comms_bridge.config @@ -37,8 +37,8 @@ links = { { -- A single link entry has required fields "from" and "to" that specify the robot roles involved -- in the link. - from = {"Bumble"}, -- manager - to = {"Queen"}, -- actor + from = "Bumble", -- manager + to = "Queen", -- actor -- The link entry has three optional fields: relay_forward (messages to be relayed only in the -- `from` to `to` direction), relay_backward (to be relayed only in the `to` to `from` direction), @@ -70,4 +70,4 @@ links = { } } -verbose = false \ No newline at end of file +verbose = 2 \ No newline at end of file diff --git a/communications/comms_bridge/src/bridge_subscriber.cpp b/communications/comms_bridge/src/bridge_subscriber.cpp index 23ca60a633..3a3ec845bc 100644 --- a/communications/comms_bridge/src/bridge_subscriber.cpp +++ b/communications/comms_bridge/src/bridge_subscriber.cpp @@ -50,20 +50,20 @@ void BridgeSubscriber::handleRelayedMessage(const ros::MessageEvent 1) { - printf(" datatype: \"%s\"\n", ptr->getDataType().c_str()); - printf(" md5: \"%s\"\n", ptr->getMD5Sum().c_str()); + ROS_INFO(" datatype: \"%s\"\n", ptr->getDataType().c_str()); + ROS_INFO(" md5: \"%s\"\n", ptr->getMD5Sum().c_str()); } if (m_verbose > 2) - printf(" def: \"%s\"\n", ptr->getMessageDefinition().c_str()); + ROS_INFO(" def: \"%s\"\n", ptr->getMessageDefinition().c_str()); if (m_verbose > 2) { - printf(" conn header:\n"); + ROS_INFO(" conn header:\n"); for (ros::M_string::const_iterator iter = connection_header->begin(); iter != connection_header->end(); iter++) - printf(" %s: %s\n", iter->first.c_str(), iter->second.c_str()); + ROS_INFO(" %s: %s\n", iter->first.c_str(), iter->second.c_str()); } } @@ -79,7 +79,7 @@ void BridgeSubscriber::handleRelayedMessage(const ros::MessageEvent 2) - printf(" serialized size = %zd\n", serialized_size); + ROS_INFO(" serialized size = %zd\n", serialized_size); if (serialized_size <= 0) { ROS_ERROR("Serialization buffer size deficient, discarding message"); return; @@ -144,7 +144,7 @@ SubscriberPtr BridgeSubscriber::rosSubscribe(std::string const& topic) { *ptr = nh.subscribe(opts); if (m_verbose) - printf("Subscribed to topic %s\n", topic.c_str()); + ROS_INFO("Subscribed to topic %s\n", topic.c_str()); return ptr; } @@ -155,7 +155,7 @@ bool BridgeSubscriber::addTopic(std::string const& in_topic, std::string const& // Enforce that all relays have a unique input topic if (m_relay_topics.find(in_topic) != m_relay_topics.end()) { if (m_verbose) - printf("Already subscribed to relay from topic %s\n", in_topic.c_str()); + ROS_ERROR("Already subscribed to relay from topic %s\n", in_topic.c_str()); return false; } @@ -166,7 +166,7 @@ bool BridgeSubscriber::addTopic(std::string const& in_topic, std::string const& while (iter != m_relay_topics.end()) { if (iter->second.out_topic == out_topic) { if (m_verbose) - printf("Already relaying to topic %s\n", out_topic.c_str()); + ROS_ERROR("Already relaying to topic %s\n", out_topic.c_str()); return false; } iter++; diff --git a/communications/comms_bridge/src/bridge_subscriber_nodelet.cpp b/communications/comms_bridge/src/bridge_subscriber_nodelet.cpp index ba5e8dc222..9416ccf1a6 100644 --- a/communications/comms_bridge/src/bridge_subscriber_nodelet.cpp +++ b/communications/comms_bridge/src/bridge_subscriber_nodelet.cpp @@ -36,11 +36,6 @@ #include "comms_bridge/ros_dds_bridge_subscriber.h" namespace comms_bridge { -// void usage(const char *progname) -// { -// fprintf(stderr, "Usage: %s [-v] [-a] [-t meta_topic_prefix] [ ...]\n", progname); -// } class BridgeSubscriberNodelet : public ff_util::FreeFlyerNodelet { public: @@ -86,17 +81,20 @@ class BridgeSubscriberNodelet : public ff_util::FreeFlyerNodelet { } } - bool verbose; - if (!config_params_.GetBool("verbose", &verbose)) { + unsigned int verbose; + if (!config_params_.GetUInt("verbose", &verbose)) { ROS_FATAL("BridgeSubscriberNodelet: Could not read verbosity level."); exit(EXIT_FAILURE); return; } - ROS_ERROR_STREAM("ROBOT NAME " << agent_name_); // Declare the ROS to DDS Subscriber class ROSDDSBridgeSubscriber sub(agent_name_); - std::string ns = "/" + std::tolower(agent_name_[0]) + agent_name_.substr(1) + "/"; + if (verbose > 0) + sub.setVerbosity(verbose); + + std::string ns = std::string("/") + agent_name_ + "/"; + ns[1] = std::tolower(ns[1]); // namespaces don't start with uppercase // Load shared topic groups config_reader::ConfigReader::Table links, link, relay_forward, relay_backward, relay_both, item_conf; @@ -121,7 +119,7 @@ class BridgeSubscriberNodelet : public ff_util::FreeFlyerNodelet { NODELET_ERROR("BridgeSubscriberNodelet: agent topic name not specified!"); return; } - assert(sub.addTopic(topic_name, ns + topic_name)); + sub.addTopic(topic_name, ns + topic_name); } } if (link.GetTable("relay_both", &relay_both)) { @@ -132,7 +130,7 @@ class BridgeSubscriberNodelet : public ff_util::FreeFlyerNodelet { NODELET_ERROR("BridgeSubscriberNodelet: agent topic name not specified!"); return; } - assert(sub.addTopic(topic_name, ns + topic_name)); + sub.addTopic(topic_name, ns + topic_name); } } } else if (link.GetStr("to", &config_agent) && config_agent == agent_name_) { @@ -144,7 +142,7 @@ class BridgeSubscriberNodelet : public ff_util::FreeFlyerNodelet { NODELET_ERROR("BridgeSubscriberNodelet: agent topic name not specified!"); return; } - assert(sub.addTopic(topic_name, ns + topic_name)); + sub.addTopic(topic_name, ns + topic_name); } } if (link.GetTable("relay_both", &relay_both)) { @@ -155,7 +153,7 @@ class BridgeSubscriberNodelet : public ff_util::FreeFlyerNodelet { NODELET_ERROR("BridgeSubscriberNodelet: agent topic name not specified!"); return; } - assert(sub.addTopic(topic_name, ns + topic_name)); + sub.addTopic(topic_name, ns + topic_name); } } } From cea5de576e79eea4223edcf3405775c4eacf5882 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Wed, 1 Nov 2023 16:26:12 -0700 Subject: [PATCH 08/32] making style more in line with astrobee + fixing mutex --- .../config/communications/comms_bridge.config | 3 +- .../include/comms_bridge/bridge_publisher.h | 16 ++-- .../include/comms_bridge/bridge_subscriber.h | 10 +-- .../comms_bridge/dds_ros_bridge_publisher.h | 2 +- .../comms_bridge/src/bridge_publisher.cpp | 68 +++++++-------- .../src/bridge_publisher_nodelet.cpp | 82 ++++++------------- .../comms_bridge/src/bridge_subscriber.cpp | 52 ++++++------ 7 files changed, 103 insertions(+), 130 deletions(-) diff --git a/astrobee/config/communications/comms_bridge.config b/astrobee/config/communications/comms_bridge.config index 4413ffff3e..331bfb079a 100644 --- a/astrobee/config/communications/comms_bridge.config +++ b/astrobee/config/communications/comms_bridge.config @@ -70,4 +70,5 @@ links = { } } -verbose = 2 \ No newline at end of file +verbose = 2 +ad2pub_delay = 3.0 \ No newline at end of file diff --git a/communications/comms_bridge/include/comms_bridge/bridge_publisher.h b/communications/comms_bridge/include/comms_bridge/bridge_publisher.h index ba484e15ad..d8a4202697 100644 --- a/communications/comms_bridge/include/comms_bridge/bridge_publisher.h +++ b/communications/comms_bridge/include/comms_bridge/bridge_publisher.h @@ -122,17 +122,17 @@ class BridgePublisher { void drainWaitingQueue(RelayTopicInfo& topic_info); void drainWaitingQueue(std::string const& output_topic); - unsigned int m_verbose; + unsigned int m_verbose_; - double m_ad2pub_delay; // seconds + double m_ad2pub_delay_; // seconds - std::mutex m_mutex; // serializes access to below data structures - std::unique_ptr worker_thread; - std::map m_relay_topics; // keyed by input topic - std::priority_queue> m_drain_queue; - std::condition_variable m_drain_cv; + std::mutex m_mutex_; // serializes access to below data structures + std::shared_ptr worker_thread_; + std::map m_relay_topics_; // keyed by input topic + std::priority_queue> m_drain_queue_; + std::condition_variable m_drain_cv_; // stats: - unsigned int m_n_relayed; + unsigned int m_n_relayed_; }; #endif // COMMS_BRIDGE_BRIDGE_PUBLISHER_H_ diff --git a/communications/comms_bridge/include/comms_bridge/bridge_subscriber.h b/communications/comms_bridge/include/comms_bridge/bridge_subscriber.h index 0ca54395aa..158d85376a 100644 --- a/communications/comms_bridge/include/comms_bridge/bridge_subscriber.h +++ b/communications/comms_bridge/include/comms_bridge/bridge_subscriber.h @@ -117,13 +117,13 @@ class BridgeSubscriber { void handleRelayedMessage(const ros::MessageEvent& msg_event, std::string const& topic, SubscriberPtr sub); - unsigned int m_verbose; + unsigned int m_verbose_; - std::mutex m_mutex; // serializes access to below data structures - uint8_t* m_msgbuffer; // data serialization scratch - std::map m_relay_topics; // keyed by input topic + std::mutex m_mutex_; // serializes access to below data structures + uint8_t* m_msgbuffer_; // data serialization scratch + std::map m_relay_topics_; // keyed by input topic // stats: - unsigned int m_n_relayed; + unsigned int m_n_relayed_; }; #endif // COMMS_BRIDGE_BRIDGE_SUBSCRIBER_H_ diff --git a/communications/comms_bridge/include/comms_bridge/dds_ros_bridge_publisher.h b/communications/comms_bridge/include/comms_bridge/dds_ros_bridge_publisher.h index 89d70a7c88..37fd2ecfd8 100644 --- a/communications/comms_bridge/include/comms_bridge/dds_ros_bridge_publisher.h +++ b/communications/comms_bridge/include/comms_bridge/dds_ros_bridge_publisher.h @@ -38,7 +38,7 @@ class DDSROSBridgePublisher : public BridgePublisher { protected: // prohibit shallow copy or assignment - DDSROSBridgePublisher(const DDSROSBridgePublisher&) : BridgePublisher(m_ad2pub_delay) {} + DDSROSBridgePublisher(const DDSROSBridgePublisher&) : BridgePublisher(m_ad2pub_delay_) {} void operator=(const DDSROSBridgePublisher&) {} }; diff --git a/communications/comms_bridge/src/bridge_publisher.cpp b/communications/comms_bridge/src/bridge_publisher.cpp index 4f4d0d63bf..48da8fdeab 100644 --- a/communications/comms_bridge/src/bridge_publisher.cpp +++ b/communications/comms_bridge/src/bridge_publisher.cpp @@ -35,28 +35,30 @@ using ros::Publisher; using ros::Publisher; using topic_tools::ShapeShifter; -BridgePublisher::BridgePublisher(double ad2pub_delay) : m_ad2pub_delay(ad2pub_delay) { - m_n_relayed = 0; - m_verbose = 0; +BridgePublisher::BridgePublisher(double ad2pub_delay) : m_ad2pub_delay_(ad2pub_delay) { + m_n_relayed_ = 0; + m_verbose_ = 0; - worker_thread = std::unique_ptr(new std::thread(&BridgePublisher::drainThread, this)); + worker_thread_.reset(new std::thread(&BridgePublisher::drainThread, this)); } -BridgePublisher::~BridgePublisher() {} +BridgePublisher::~BridgePublisher() { + worker_thread_->join(); +} -void BridgePublisher::setVerbosity(unsigned int verbosity) { m_verbose = verbosity; } +void BridgePublisher::setVerbosity(unsigned int verbosity) { m_verbose_ = verbosity; } bool BridgePublisher::advertiseTopic(const std::string& output_topic, const AdvertisementInfo& ad_info) { - if (m_verbose) + if (m_verbose_) printf("Advertising for topic %s\n", output_topic.c_str()); - std::map::iterator iter = m_relay_topics.find(output_topic); - if (iter == m_relay_topics.end()) { - if (m_verbose) + std::map::iterator iter = m_relay_topics_.find(output_topic); + if (iter == m_relay_topics_.end()) { + if (m_verbose_) printf(" topic is yet-unknown\n"); RelayTopicInfo topic_info; topic_info.out_topic = output_topic; - iter = m_relay_topics.emplace(output_topic, topic_info).first; + iter = m_relay_topics_.emplace(output_topic, topic_info).first; } RelayTopicInfo &topic_info = iter->second; @@ -86,11 +88,11 @@ bool BridgePublisher::advertiseTopic(const std::string& output_topic, const Adve topic_info.advertised = true; const timepoint_t now = std::chrono::steady_clock::now(); - topic_info.delay_until = now + std::chrono::microseconds((unsigned)(m_ad2pub_delay * 1e6)); + topic_info.delay_until = now + std::chrono::microseconds((unsigned)(m_ad2pub_delay_ * 1e6)); - if (m_ad2pub_delay > 0) { - m_drain_queue.push(std::make_pair(topic_info.delay_until, output_topic)); - m_drain_cv.notify_all(); + if (m_ad2pub_delay_ > 0) { + m_drain_queue_.push(std::make_pair(topic_info.delay_until, output_topic)); + m_drain_cv_.notify_all(); } else { // no post-advertise delay @@ -128,20 +130,20 @@ bool BridgePublisher::relayContent(RelayTopicInfo& topic_info, const ContentInfo } topic_info.relay_seqnum++; - m_n_relayed++; + m_n_relayed_++; - if (m_verbose) + if (m_verbose_) printf("Sent message on topic %s\n", topic_info.out_topic.c_str()); return true; } bool BridgePublisher::relayMessage(RelayTopicInfo& topic_info, const ContentInfo& content_info) { - if (m_verbose) + if (m_verbose_) printf("Relaying content message for topic %s\n", topic_info.out_topic.c_str()); if (!topic_info.advertised) { - if (m_verbose) + if (m_verbose_) printf(" topic is yet-unknown, enqueuing message for now\n"); topic_info.waiting_msgs.emplace(content_info); @@ -161,7 +163,7 @@ bool BridgePublisher::relayMessage(RelayTopicInfo& topic_info, const ContentInfo // we're still delaying after advertising // so just enqueue messages - if (m_verbose) + if (m_verbose_) printf(" topic is not %s, enqueuing message for now\n", (!topic_info.advertised ? "yet advertised" : "yet ready")); @@ -171,7 +173,7 @@ bool BridgePublisher::relayMessage(RelayTopicInfo& topic_info, const ContentInfo topic_info.waiting_msgs.emplace(content_info); - if (m_verbose > 1) + if (m_verbose_ > 1) printf(" %zu messages now waiting\n", topic_info.waiting_msgs.size()); return true; @@ -185,29 +187,29 @@ bool BridgePublisher::relayMessage(RelayTopicInfo& topic_info, const ContentInfo } void BridgePublisher::drainThread() { - std::unique_lock lk(m_mutex); + std::unique_lock lk(m_mutex_); - if (m_verbose) - printf("Started drain thread\n"); + if (m_verbose_) + ROS_INFO("Started drain thread\n"); while (1) { - while (m_drain_queue.empty()) - m_drain_cv.wait(lk); + while (m_drain_queue_.empty()) + m_drain_cv_.wait(lk); - std::pair entry = m_drain_queue.top(); - m_drain_queue.pop(); + std::pair entry = m_drain_queue_.top(); + m_drain_queue_.pop(); lk.unlock(); timepoint_t t = entry.first; - if (m_verbose > 1) + if (m_verbose_ > 1) printf( "draining %s in %0.3f seconds\n", entry.second.c_str(), std::chrono::duration((t - std::chrono::steady_clock::now())).count()); std::this_thread::sleep_until(t); - if (m_verbose > 1) + if (m_verbose_ > 1) printf("draining %s now\n", entry.second.c_str()); lk.lock(); @@ -218,7 +220,7 @@ void BridgePublisher::drainThread() { // called with mutex held void BridgePublisher::drainWaitingQueue(RelayTopicInfo& topic_info) { - if (m_verbose && topic_info.waiting_msgs.size() > 0) + if (m_verbose_ && topic_info.waiting_msgs.size() > 0) printf("Draining queue of %zu waiting messages\n", topic_info.waiting_msgs.size()); while (topic_info.waiting_msgs.size() > 0) { const ContentInfo old_msg = topic_info.waiting_msgs.front(); @@ -229,8 +231,8 @@ void BridgePublisher::drainWaitingQueue(RelayTopicInfo& topic_info) { // called with mutex held void BridgePublisher::drainWaitingQueue(std::string const& output_topic) { - std::map::iterator iter = m_relay_topics.find(output_topic); - assert(iter != m_relay_topics.end()); + std::map::iterator iter = m_relay_topics_.find(output_topic); + assert(iter != m_relay_topics_.end()); RelayTopicInfo &topic_info = iter->second; diff --git a/communications/comms_bridge/src/bridge_publisher_nodelet.cpp b/communications/comms_bridge/src/bridge_publisher_nodelet.cpp index d2d8b615f5..ccc44da582 100644 --- a/communications/comms_bridge/src/bridge_publisher_nodelet.cpp +++ b/communications/comms_bridge/src/bridge_publisher_nodelet.cpp @@ -36,12 +36,6 @@ namespace comms_bridge { -// void usage(const char *progname) -// { -// fprintf(stderr, "Usage: %s [-v] [-a] [-d ] [-t meta_topic_prefix]\n", -// progname); -// } - class BridgePublisherNodelet : public ff_util::FreeFlyerNodelet { public: BridgePublisherNodelet() : ff_util::FreeFlyerNodelet("comms_bridge_pub") {} @@ -50,59 +44,35 @@ class BridgePublisherNodelet : public ff_util::FreeFlyerNodelet { protected: virtual void Initialize(ros::NodeHandle* nh) { - // bool anonymous_node = false; - // unsigned int verbose = 0; - // std::string meta_topic_prefix = "/polymorph_relay"; - // double ad2pub_delay = 3.0; - - // int c; - // while ((c = getopt(argc, argv, "avd:t:")) != -1) { - // switch (c) { - // case 'a': - // anonymous_node = true; - // break; - // case 'v': - // verbose++; - // break; - // case 'd': - // ad2pub_delay = atof(optarg); - // if (ad2pub_delay < 0 || std::isnan(ad2pub_delay)) { - // fprintf(stderr, "Invalid advertise delay %s\n", optarg); - // usage(argv[0]); - // return 1; - // } - // break; - // case 't': - // meta_topic_prefix = optarg; - // break; - // case '?': - // default: - // usage(argv[0]); - // return 1; - // break; - // } - // } - - // if (argc-optind != 0) { - // usage(argv[0]); - // return 1; - // } - - // ros::init(argc, argv, "polymorph_relay_pub", (anonymous_node ? ros::init_options::AnonymousName : 0)); - // ros::NodeHandle nhp("~"); - // //nhp.param("verbose", verbose, 0); - - // ROSROSBridgePublisher pub(meta_topic_prefix, ad2pub_delay); + config_params_.AddFile("communications/comms_bridge.config"); + + if (!config_params_.ReadFiles()) { + ROS_FATAL("BridgePublisherNodelet: Error reading config files."); + exit(EXIT_FAILURE); + return; + } + + unsigned int verbose; + if (!config_params_.GetUInt("verbose", &verbose)) { + ROS_FATAL("BridgePublisherNodelet: Could not read verbosity level."); + exit(EXIT_FAILURE); + return; + } + + double ad2pub_delay = 3.0; + if (!config_params_.GetReal("ad2pub_delay", &ad2pub_delay)) { + ROS_FATAL("BridgePublisherNodelet: Could not read advertiser to publisher delay."); + exit(EXIT_FAILURE); + return; + } + + DDSROSBridgePublisher pub(ad2pub_delay); // if (verbose > 0) // pub.setVerbosity(verbose); - - // printf("Waiting for messages\n"); - // ros::spin(); - - // printf("Exiting\n"); - - // return 0; } + + private: + config_reader::ConfigReader config_params_; }; PLUGINLIB_EXPORT_CLASS(comms_bridge::BridgePublisherNodelet, nodelet::Nodelet) diff --git a/communications/comms_bridge/src/bridge_subscriber.cpp b/communications/comms_bridge/src/bridge_subscriber.cpp index 3a3ec845bc..577abdfceb 100644 --- a/communications/comms_bridge/src/bridge_subscriber.cpp +++ b/communications/comms_bridge/src/bridge_subscriber.cpp @@ -32,15 +32,15 @@ using ros::Publisher; using topic_tools::ShapeShifter; BridgeSubscriber::BridgeSubscriber() { - m_n_relayed = 0; - m_verbose = 0; + m_n_relayed_ = 0; + m_verbose_ = 0; - m_msgbuffer = new uint8_t[ROS_BRIDGE_MAX_MSG_SIZE]; + m_msgbuffer_ = new uint8_t[ROS_BRIDGE_MAX_MSG_SIZE]; } -BridgeSubscriber::~BridgeSubscriber() { delete[] m_msgbuffer; } +BridgeSubscriber::~BridgeSubscriber() { delete[] m_msgbuffer_; } -void BridgeSubscriber::setVerbosity(unsigned int verbosity) { m_verbose = verbosity; } +void BridgeSubscriber::setVerbosity(unsigned int verbosity) { m_verbose_ = verbosity; } void BridgeSubscriber::handleRelayedMessage(const ros::MessageEvent& msg_event, std::string const& topic, SubscriberPtr sub) { @@ -49,16 +49,16 @@ void BridgeSubscriber::handleRelayedMessage(const ros::MessageEvent const& connection_header = msg_event.getConnectionHeaderPtr(); - if (m_verbose) { + if (m_verbose_) { ROS_INFO("got data on %s:\n", topic.c_str()); - if (m_verbose > 1) { + if (m_verbose_ > 1) { ROS_INFO(" datatype: \"%s\"\n", ptr->getDataType().c_str()); ROS_INFO(" md5: \"%s\"\n", ptr->getMD5Sum().c_str()); } - if (m_verbose > 2) + if (m_verbose_ > 2) ROS_INFO(" def: \"%s\"\n", ptr->getMessageDefinition().c_str()); - if (m_verbose > 2) { + if (m_verbose_ > 2) { ROS_INFO(" conn header:\n"); for (ros::M_string::const_iterator iter = connection_header->begin(); iter != connection_header->end(); @@ -67,10 +67,10 @@ void BridgeSubscriber::handleRelayedMessage(const ros::MessageEvent lock(m_mutex); + std::unique_lock lock(m_mutex_); - std::map::iterator iter = m_relay_topics.find(topic); - if (iter == m_relay_topics.end()) { + std::map::iterator iter = m_relay_topics_.find(topic); + if (iter == m_relay_topics_.end()) { printf("Received message on non-relayed topic %s\n", topic.c_str()); return; } @@ -78,7 +78,7 @@ void BridgeSubscriber::handleRelayedMessage(const ros::MessageEventsecond; if (!topic_info.advertised) { - if (m_verbose) + if (m_verbose_) ROS_INFO(" sending advertisement\n"); bool latching = false; @@ -102,10 +102,10 @@ void BridgeSubscriber::handleRelayedMessage(const ros::MessageEvent 2) + if (m_verbose_ > 2) ROS_INFO(" serialized size = %zd\n", serialized_size); if (serialized_size <= 0) { ROS_ERROR("Serialization buffer size deficient, discarding message"); @@ -115,15 +115,15 @@ void BridgeSubscriber::handleRelayedMessage(const ros::MessageEventgetMD5Sum(); content_info.data_size = (size_t)serialized_size; - content_info.data = m_msgbuffer; + content_info.data = m_msgbuffer_; relayMessage(topic_info, content_info); topic_info.relay_seqnum++; - m_n_relayed++; + m_n_relayed_++; lock.release()->unlock(); // now done with any access to topic info - if (m_verbose) + if (m_verbose_) fflush(stdout); } @@ -143,18 +143,18 @@ SubscriberPtr BridgeSubscriber::rosSubscribe(std::string const& topic) { std::bind(&BridgeSubscriber::handleRelayedMessage, this, std::placeholders::_1, topic, ptr)); *ptr = nh.subscribe(opts); - if (m_verbose) + if (m_verbose_) ROS_INFO("Subscribed to topic %s\n", topic.c_str()); return ptr; } bool BridgeSubscriber::addTopic(std::string const& in_topic, std::string const& out_topic) { - const std::lock_guard lock(m_mutex); + const std::lock_guard lock(m_mutex_); // Enforce that all relays have a unique input topic - if (m_relay_topics.find(in_topic) != m_relay_topics.end()) { - if (m_verbose) + if (m_relay_topics_.find(in_topic) != m_relay_topics_.end()) { + if (m_verbose_) ROS_ERROR("Already subscribed to relay from topic %s\n", in_topic.c_str()); return false; } @@ -162,10 +162,10 @@ bool BridgeSubscriber::addTopic(std::string const& in_topic, std::string const& // Enforce that all relays have a unique output topic // The republishing side will already be checking for this but may have no way // to communicate that to us, so we check too - std::map::iterator iter = m_relay_topics.begin(); - while (iter != m_relay_topics.end()) { + std::map::iterator iter = m_relay_topics_.begin(); + while (iter != m_relay_topics_.end()) { if (iter->second.out_topic == out_topic) { - if (m_verbose) + if (m_verbose_) ROS_ERROR("Already relaying to topic %s\n", out_topic.c_str()); return false; } @@ -178,7 +178,7 @@ bool BridgeSubscriber::addTopic(std::string const& in_topic, std::string const& topic_info.sub = rosSubscribe(in_topic); // handleRelayedMessage() may immediately start getting called - m_relay_topics[in_topic] = topic_info; + m_relay_topics_[in_topic] = topic_info; // let implementation know subscribeTopic(in_topic, topic_info); From 702ffdaa141e14de5b3ee6309993d3855a385460 Mon Sep 17 00:00:00 2001 From: Katie Hamilton Date: Fri, 10 Nov 2023 13:40:55 -0800 Subject: [PATCH 09/32] Initial generic bridge dds work. --- .../dds_generic_comms/NDDS_DISCOVERY_PEERS | 17 + .../dds_generic_comms/RAPID_QOS_PROFILES.xml | 994 ++++++++++++++++++ communications/comms_bridge/CMakeLists.txt | 40 +- .../comms_bridge/dds_ros_bridge_publisher.h | 7 +- .../comms_bridge/generic_ros_sub_dds_pub.h | 81 ++ .../comms_bridge/ros_dds_bridge_subscriber.h | 65 -- .../comms_bridge/include/comms_bridge/util.h | 54 + .../comms_bridge/nodelet_plugins.xml | 13 +- communications/comms_bridge/package.xml | 4 + .../src/bridge_subscriber_nodelet.cpp | 170 --- .../comms_bridge/src/comms_bridge_nodelet.cpp | 275 +++++ .../src/generic_ros_sub_dds_pub.cpp | 104 ++ .../src/ros_dds_bridge_subscriber.cpp | 111 -- communications/comms_bridge/src/util.cc | 51 + .../dds_msgs/idl/AstrobeeConstants.idl | 3 + .../idl/GenericCommsAdvertisementInfo.idl | 45 + .../dds_msgs/idl/GenericCommsContent.idl | 39 + .../dds_msgs/idl/GenericCommsReset.idl | 33 + ....msg => GenericCommsAdvertisementInfo.msg} | 0 ...layContent.msg => GenericCommsContent.msg} | 0 .../{RelayReset.msg => GenericCommsReset.msg} | 0 21 files changed, 1728 insertions(+), 378 deletions(-) create mode 100644 astrobee/config/communications/dds_generic_comms/NDDS_DISCOVERY_PEERS create mode 100644 astrobee/config/communications/dds_generic_comms/RAPID_QOS_PROFILES.xml create mode 100644 communications/comms_bridge/include/comms_bridge/generic_ros_sub_dds_pub.h delete mode 100644 communications/comms_bridge/include/comms_bridge/ros_dds_bridge_subscriber.h create mode 100644 communications/comms_bridge/include/comms_bridge/util.h delete mode 100644 communications/comms_bridge/src/bridge_subscriber_nodelet.cpp create mode 100644 communications/comms_bridge/src/comms_bridge_nodelet.cpp create mode 100644 communications/comms_bridge/src/generic_ros_sub_dds_pub.cpp delete mode 100644 communications/comms_bridge/src/ros_dds_bridge_subscriber.cpp create mode 100644 communications/comms_bridge/src/util.cc create mode 100644 communications/dds_msgs/idl/GenericCommsAdvertisementInfo.idl create mode 100644 communications/dds_msgs/idl/GenericCommsContent.idl create mode 100644 communications/dds_msgs/idl/GenericCommsReset.idl rename communications/ff_msgs/msg/{RelayAdvertisement.msg => GenericCommsAdvertisementInfo.msg} (100%) rename communications/ff_msgs/msg/{RelayContent.msg => GenericCommsContent.msg} (100%) rename communications/ff_msgs/msg/{RelayReset.msg => GenericCommsReset.msg} (100%) diff --git a/astrobee/config/communications/dds_generic_comms/NDDS_DISCOVERY_PEERS b/astrobee/config/communications/dds_generic_comms/NDDS_DISCOVERY_PEERS new file mode 100644 index 0000000000..0f7f694397 --- /dev/null +++ b/astrobee/config/communications/dds_generic_comms/NDDS_DISCOVERY_PEERS @@ -0,0 +1,17 @@ +;; NDDS_DISCOVERY_PEERS - Default Discovery Configuration File +;;Multicast builtin.udpv4://239.255.0.1 ; default discovery multicast addr +; EVERY LINE IN THIS FILE MUST END IN A COMMENT. Even blank lines. +; +;; Shared Memory +;shmem:// ; All 'shmem' transport plugin(s) +;builtin.shmem:// ; The builtin builtin 'shmem' transport plugin + ; at network address FCC0::0 +; +;; Unicast +; +;; Localhost +127.0.0.1 ; A comma can be used a separator +; +;; Robot to communicate with +; +;; Custom entries below this line (Vagrant, etc) diff --git a/astrobee/config/communications/dds_generic_comms/RAPID_QOS_PROFILES.xml b/astrobee/config/communications/dds_generic_comms/RAPID_QOS_PROFILES.xml new file mode 100644 index 0000000000..78ddb509a0 --- /dev/null +++ b/astrobee/config/communications/dds_generic_comms/RAPID_QOS_PROFILES.xml @@ -0,0 +1,994 @@ + + + + + + + + + + + + + + + 50, 48, 49, 49, 48, 54,0x32,0x39,0x2e,0x30,0x39,0x34,0x33,0x30,0x30,0x2e,0x48,0x55,0x2d,0x41,0x52,0x43 + + + + + 7400 + 250 + 2 + 0 + 10 + 1 + 11 + + MASK_DEFAULT + + + UDPv4 + + + + + + true + + + + 4096 + + + 0 + 0 + + + + + + + + + + + dds.flow_controller.token_bucket.MeshFlowController.scheduling_policy + DDS_EDF_FLOW_CONTROLLER_SCHED_POLICY + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.max_tokens + 12 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.tokens_added_per_period + 4 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.tokens_leaked_per_period + 0 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.bytes_per_token + 1250 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.period.sec + 0 + + + dds.flow_controller.token_bucket.MeshFlowController.token_bucket.period.nanosec + 5000000 + + + + + + + dds.transport.UDPv4.builtin.parent.message_size_max + 1250 + + + dds.transport.UDPv4.builtin.send_socket_buffer_size + 65535 + + + dds.transport.UDPv4.builtin.recv_socket_buffer_size + 65535 + + + + + dds.transport.shmem.builtin.received_message_count_max + 512 + + + dds.transport.shmem.builtin.receive_buffer_size + 1048576 + + + + + dds.transport.UDPv4.builtin.parent.deny_interfaces + 128.102.*,10.10.1.*,10.30.10.*,192.168.3.255,169.254.194.2,192.9.202.1,192.168.5.*,192.168.10.*,192.168.122.* + + + dds.transport.UDPv4.builtin.parent.deny_multicast_interfaces + 128.102.*,10.10.1.*,10.30.10.*,192.168.3.255,169.254.194.2,192.9.202.1,192.168.5.*,192.168.10.*,192.168.122.* + + + + + + + + + + + + 0 + + + + + + ALIVE_THEN_DISPOSED_INSTANCE_REPLACEMENT + + + ASYNCHRONOUS_PUBLISH_MODE_QOS + + + + + + + + + + BEST_EFFORT_RELIABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 5 + + + + + BEST_EFFORT_RELIABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 5 + + + + + + + + + + RELIABLE_RELIABILITY_QOS + + + KEEP_ALL_HISTORY_QOS + + + + + RELIABLE_RELIABILITY_QOS + + + + KEEP_ALL_HISTORY_QOS + + + + + + + + + + RELIABLE_RELIABILITY_QOS + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + RELIABLE_RELIABILITY_QOS + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + RELIABLE_RELIABILITY_QOS + + + + + RELIABLE_RELIABILITY_QOS + + + + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 6 + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 6 + + + + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + + + + KEEP_LAST_HISTORY_QOS + 1 + + + 512 + 4 + 128 + 128 + 512 + 380 + + + + 32 + + 0 + 500000000 + + 8 + true + + + + + + + + + + + + + + + KEEP_LAST_HISTORY_QOS + 10 + + + + + KEEP_LAST_HISTORY_QOS + 100 + + + + + + + + + + + + + + + + + + + KEEP_LAST_HISTORY_QOS + 1000 + + + + + KEEP_LAST_HISTORY_QOS + 200 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + EXCLUSIVE_OWNERSHIP_QOS + + + 100 + + + + + EXCLUSIVE_OWNERSHIP_QOS + + + + + + + + + 50 + + + + + + + + + + + + + + + + 1000 + + + + + + + + 1000 + + + + + + + + + + + + + + + + + + + dds.flow_controller.token_bucket.MeshFlowController + + + KEEP_LAST_HISTORY_QOS + 1 + + + 32 + 32 + 1 + 1 + 32 + + + + + 4096 + + + 4 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 10 + 0 + + + + + + + 10 + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 10 + 0 + + + + + + + 10 + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + KEEP_LAST_HISTORY_QOS + 5 + + + + + KEEP_LAST_HISTORY_QOS + 5 + + + + + + + + + + + + + + + + + + + + + + + + 7400 + 250 + 2 + 0 + 10 + 1 + 11 + + + + + 30000 + 64 + 2048 + + + + UDPv4 | SHMEM + + + Monitoring UI Application + + + 65530 + + + + + 100 + 0 + + + 10 + 0 + + LIVELINESS_BASED_REMOTE_PARTICIPANT_PURGE + + 10 + 0 + + 1 + + 2 + 0 + + + 2 + 0 + + + + 0 + 0 + + + 0 + 0 + + + 0 + 0 + + + 0 + 100000000 + + + + + 0 + 0 + + + 0 + 0 + + + 0 + 0 + + + 0 + 100000000 + + + + 0 + 1 + + 5 + 0 + + + 0 + 200000000 + + + 0 + 200000000 + + 1000 + 8 + + 0 + 0 + + + 0 + 0 + + + 0 + 0 + + 65530 + + + 0 + 1 + + 5 + 0 + + + 0 + 200000000 + + + 0 + 200000000 + + 1000 + 8 + + 0 + 0 + + + 0 + 0 + + + 0 + 0 + + 65530 + + + + + + + dds.transport.UDPv4.builtin.recv_socket_buffer_size + 1048576 + + + + dds.transport.UDPv4.builtin.parent.message_size_max + 65530 + + + dds.transport.UDPv4.builtin.send_socket_buffer_size + 65530 + + + + + dds.transport.shmem.builtin.parent.message_size_max + 65530 + + + dds.transport.shmem.builtin.receive_buffer_size + 65530 + + + dds.transport.shmem.builtin.received_message_count_max + 32 + + + + + + + + + + + + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + + RELIABLE_RELIABILITY_QOS + + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + + + + + BEST_EFFORT_RELIABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 2 + + + + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + + RELIABLE_RELIABILITY_QOS + + + + KEEP_LAST_HISTORY_QOS + 1 + + + + + + + KEEP_LAST_HISTORY_QOS + 10 + + + + + + + KEEP_LAST_HISTORY_QOS + 10 + + + + + diff --git a/communications/comms_bridge/CMakeLists.txt b/communications/comms_bridge/CMakeLists.txt index 6af17b55b6..c60636a945 100644 --- a/communications/comms_bridge/CMakeLists.txt +++ b/communications/comms_bridge/CMakeLists.txt @@ -32,9 +32,11 @@ find_package(catkin2 REQUIRED COMPONENTS image_transport std_msgs sensor_msgs + config_reader ff_util ff_msgs topic_tools + dds_msgs ) @@ -70,10 +72,8 @@ find_package(RtiDds REQUIRED) find_package(Soracore REQUIRED) catkin_package( - LIBRARIES src - CATKIN_DEPENDS - message_runtime - std_msgs + LIBRARIES comms_bridge + CATKIN_DEPENDS message_runtime std_msgs nodelet config_reader ff_util ff_msgs dds_msgs ) ########### @@ -94,31 +94,31 @@ include_directories( # Declare C++ libraries -add_library(comms_bridge_sub +add_library(comms_bridge src/bridge_subscriber.cpp - src/ros_dds_bridge_subscriber.cpp - src/bridge_subscriber_nodelet.cpp + src/generic_ros_sub_dds_pub.cpp + src/comms_bridge_nodelet.cpp ) -add_dependencies(comms_bridge_sub ${catkin_EXPORTED_TARGETS}) -target_link_libraries(comms_bridge_sub - rapidIo Qt5::Xml ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${CMAKE_DL_LIBS} ) +target_compile_definitions(comms_bridge PUBLIC ${RTIDDS_DEFINE_FLAGS}) +add_dependencies(comms_bridge ${catkin_EXPORTED_TARGETS}) +target_link_libraries(comms_bridge rapidIo Qt5::Xml ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -add_library(comms_bridge_pub - src/bridge_publisher.cpp - src/dds_ros_bridge_publisher.cpp - src/bridge_publisher_nodelet.cpp -) -add_dependencies(comms_bridge_pub ${catkin_EXPORTED_TARGETS} ) -target_link_libraries(comms_bridge_pub - rapidIo Qt5::Xml ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${CMAKE_DL_LIBS} ) +#add_library(comms_bridge_pub +# src/bridge_publisher.cpp +# src/dds_ros_bridge_publisher.cpp +# src/bridge_publisher_nodelet.cpp +#) +#add_dependencies(comms_bridge_pub ${catkin_EXPORTED_TARGETS} ) +#target_link_libraries(comms_bridge_pub +# rapidIo Qt5::Xml ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${CMAKE_DL_LIBS} ) ############# ## Install ## ############# # Mark libraries for installation -install(TARGETS ${PROJECT_NAME}_sub ${PROJECT_NAME}_pub +install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} @@ -137,4 +137,4 @@ install(DIRECTORY launch/ else (USE_DDS) find_package(catkin REQUIRED COMPONENTS) catkin_package() -endif (USE_DDS) \ No newline at end of file +endif (USE_DDS) diff --git a/communications/comms_bridge/include/comms_bridge/dds_ros_bridge_publisher.h b/communications/comms_bridge/include/comms_bridge/dds_ros_bridge_publisher.h index 37fd2ecfd8..cc01668c07 100644 --- a/communications/comms_bridge/include/comms_bridge/dds_ros_bridge_publisher.h +++ b/communications/comms_bridge/include/comms_bridge/dds_ros_bridge_publisher.h @@ -22,9 +22,9 @@ /* This is a specialization of ROSBridgePublisher using a DDS conduit */ -#include -#include -#include +#include +#include +#include #include @@ -33,6 +33,7 @@ class DDSROSBridgePublisher : public BridgePublisher { public: + // explicit DDSROSBridgePublisher(); explicit DDSROSBridgePublisher(double ad2pub_delay = DEFAULT_DDSROSBRIDGE_PUB_ADVERTISE_DELAY); virtual ~DDSROSBridgePublisher(); diff --git a/communications/comms_bridge/include/comms_bridge/generic_ros_sub_dds_pub.h b/communications/comms_bridge/include/comms_bridge/generic_ros_sub_dds_pub.h new file mode 100644 index 0000000000..0ec7817e7c --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/generic_ros_sub_dds_pub.h @@ -0,0 +1,81 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_GENERIC_ROS_SUB_DDS_PUB_H_ +#define COMMS_BRIDGE_GENERIC_ROS_SUB_DDS_PUB_H_ + +/* This is a specialization of ROSBridgeSubscriber using a DDS conduit +*/ + +#include +#include + +#include +#include +#include + +#include + +#include "knDds/DdsTypedSupplier.h" + +#include "rapidUtil/RapidHelper.h" + +#include "dds_msgs/AstrobeeConstantsSupport.h" +#include "dds_msgs/GenericCommsAdvertisementInfoSupport.h" +#include "dds_msgs/GenericCommsContentSupport.h" + +class GenericROSSubDDSPub : public BridgeSubscriber { + public: + GenericROSSubDDSPub(); + ~GenericROSSubDDSPub(); + + void InitializeDDS(std::string agent_name); + void SizeCheck(unsigned int &size, + const int size_in, + const int max_size, + std::string const& data_name, + std::string const& topic); + + // Called with the mutex held + virtual void subscribeTopic(std::string const& in_topic, const RelayTopicInfo& info); + + // Called with the mutex held + virtual void advertiseTopic(const RelayTopicInfo& info); + + // Called with the mutex held + virtual void relayMessage(const RelayTopicInfo& topic_info, ContentInfo const& content_info); + + private: + bool dds_initialized_; + + using AdvertisementInfoSupplier = + kn::DdsTypedSupplier; + using AdvertisementInfoSupplierPtr = std::unique_ptr; + + AdvertisementInfoSupplierPtr advertisement_info_supplier_; + + using ContentSupplier = + kn::DdsTypedSupplier; + using ContentSupplierPtr = std::unique_ptr; + + ContentSupplierPtr content_supplier_; + + unsigned int advertisement_info_seq_; +}; + +#endif // COMMS_BRIDGE_GENERIC_ROS_SUB_DDS_PUB_H_ diff --git a/communications/comms_bridge/include/comms_bridge/ros_dds_bridge_subscriber.h b/communications/comms_bridge/include/comms_bridge/ros_dds_bridge_subscriber.h deleted file mode 100644 index 83fa348490..0000000000 --- a/communications/comms_bridge/include/comms_bridge/ros_dds_bridge_subscriber.h +++ /dev/null @@ -1,65 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef COMMS_BRIDGE_ROS_DDS_BRIDGE_SUBSCRIBER_H_ -#define COMMS_BRIDGE_ROS_DDS_BRIDGE_SUBSCRIBER_H_ - -/* This is a specialization of ROSBridgeSubscriber using a DDS conduit -*/ - -#include -#include -#include - -#include - -// // SoraCore Includes -// #include "knDds/DdsSupport.h" -// #include "knDds/DdsEntitiesFactory.h" -// #include "knDds/DdsEntitiesFactorySvc.h" - -// miro includes -#include -#include -#include - -#include - -class ROSDDSBridgeSubscriber : public BridgeSubscriber { - public: - explicit ROSDDSBridgeSubscriber(std::string agent_name); - virtual ~ROSDDSBridgeSubscriber(); - - // Called with the mutex held - virtual void subscribeTopic(std::string const& in_topic, const RelayTopicInfo& info); - - // Called with the mutex held - virtual void advertiseTopic(const RelayTopicInfo& info); - - // Called with the mutex held - virtual void relayMessage(const RelayTopicInfo& topic_info, ContentInfo const& content_info); - - protected: - // prohibit shallow copy or assignment - ROSDDSBridgeSubscriber(const ROSDDSBridgeSubscriber&) {} - void operator=(const ROSDDSBridgeSubscriber&) {} - private: - std::string agent_name_, participant_name_; -}; - -#endif // COMMS_BRIDGE_ROS_DDS_BRIDGE_SUBSCRIBER_H_ diff --git a/communications/comms_bridge/include/comms_bridge/util.h b/communications/comms_bridge/include/comms_bridge/util.h new file mode 100644 index 0000000000..51c07eda52 --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/util.h @@ -0,0 +1,54 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_UTIL_H_ +#define COMMS_BRIDGE_UTIL_H_ + +#include + +#include +#include + +#include + +#include + +#include + +#include + +namespace comms_util { + + // Time-related helpers + ::ros::Time RapidTime2RosTime(const int64_t dds_time); + + int64_t RosTime2RapidTime(::ros::Time const& ros_time); + + // Header-related helpers + void RosHeader2Rapid(::std_msgs::Header const& ros_hdr, + ::rapid::Header* rapid_hdr, + int status = 0, + int serial = -1); + + void RapidHeader2Ros(::rapid::Header const& rapid_hdr, + ::std_msgs::Header* ros_hdr, + std::string const& frame_id = "world"); + +} // end namespace comms_util + +#endif // COMMS_BRIDGE_UTIL_H_ diff --git a/communications/comms_bridge/nodelet_plugins.xml b/communications/comms_bridge/nodelet_plugins.xml index 693aea970d..f30810f612 100644 --- a/communications/comms_bridge/nodelet_plugins.xml +++ b/communications/comms_bridge/nodelet_plugins.xml @@ -1,13 +1,8 @@ - - + - Nodelet for the comms bridge subscriber + Nodelet for the comms bridge - - - Nodelet for the comms bridge publisher - - - \ No newline at end of file + diff --git a/communications/comms_bridge/package.xml b/communications/comms_bridge/package.xml index 11097a16b5..df3076380b 100644 --- a/communications/comms_bridge/package.xml +++ b/communications/comms_bridge/package.xml @@ -25,6 +25,8 @@ ff_util ff_msgs topic_tools + config_reader + dds_msgs roscpp nodelet message_generation @@ -35,6 +37,8 @@ ff_util ff_msgs topic_tools + config_reader + dds_msgs diff --git a/communications/comms_bridge/src/bridge_subscriber_nodelet.cpp b/communications/comms_bridge/src/bridge_subscriber_nodelet.cpp deleted file mode 100644 index 9416ccf1a6..0000000000 --- a/communications/comms_bridge/src/bridge_subscriber_nodelet.cpp +++ /dev/null @@ -1,170 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -// Standard ROS includes -#include -#include -#include - -// FSW shared libraries -#include - -// FSW nodelet -#include -#include - -#include -#include - -#include - -#include "comms_bridge/ros_dds_bridge_subscriber.h" - -namespace comms_bridge { - -class BridgeSubscriberNodelet : public ff_util::FreeFlyerNodelet { - public: - BridgeSubscriberNodelet() : ff_util::FreeFlyerNodelet("comms_bridge_sub") {} - - virtual ~BridgeSubscriberNodelet() {} - - protected: - virtual void Initialize(ros::NodeHandle* nh) { - // Need to get robot name which is in the lua config files, add files, read - // files, and get robot name but don't get the rest of the config parameters - // since these params are used to create the classes that use rapid dds. Need - // to set up Miro/DDs before reading the parameters. - - config_params_.AddFile("communications/comms_bridge.config"); - - if (!config_params_.ReadFiles()) { - ROS_FATAL("BridgeSubscriberNodelet: Error reading config files."); - exit(EXIT_FAILURE); - return; - } - - if (!config_params_.GetStr("agent_name", &agent_name_)) { - ROS_FATAL("BridgeSubscriberNodelet: Could not read robot name."); - exit(EXIT_FAILURE); - return; - } - - // In simulation, the namespace is usually set to the robot name so we need to - // check if we are in simulation and get the right name - if (agent_name_ == "sim" || agent_name_ == "simulator") { - // The platform name should be the simulated robot name - agent_name_ = GetPlatform(); - - // If there is not robot name, set it to a default name so that we can - // connect to the bridge - if (agent_name_ == "") { - agent_name_ = "Bumble"; - } else { - // Make sure that first letter of robot name is capitialized. GDS only - // recognizes capitialized robot names. - agent_name_[0] = toupper(agent_name_[0]); - } - } - - unsigned int verbose; - if (!config_params_.GetUInt("verbose", &verbose)) { - ROS_FATAL("BridgeSubscriberNodelet: Could not read verbosity level."); - exit(EXIT_FAILURE); - return; - } - - // Declare the ROS to DDS Subscriber class - ROSDDSBridgeSubscriber sub(agent_name_); - if (verbose > 0) - sub.setVerbosity(verbose); - - std::string ns = std::string("/") + agent_name_ + "/"; - ns[1] = std::tolower(ns[1]); // namespaces don't start with uppercase - - // Load shared topic groups - config_reader::ConfigReader::Table links, link, relay_forward, relay_backward, relay_both, item_conf; - if (!config_params_.GetTable("links", &links)) { - ROS_FATAL("BridgeSubscriberNodelet: Links not specified!"); - return; - } - - // Need to search for the collision distance in the mapper parameters - for (int i = 1; i <= links.GetSize(); i++) { - if (!links.GetTable(i, &link)) { - NODELET_ERROR("Could not read the mapper parameter table row %i", i); - continue; - } - std::string config_agent, topic_name; - if (link.GetStr("from", &config_agent) && config_agent == agent_name_) { - if (link.GetTable("relay_forward", &relay_forward)) { - for (int j = 1; j <= relay_forward.GetSize(); j++) { - // Agent topic configuration - relay_forward.GetTable(j, &item_conf); - if (!item_conf.GetStr("name", &topic_name)) { - NODELET_ERROR("BridgeSubscriberNodelet: agent topic name not specified!"); - return; - } - sub.addTopic(topic_name, ns + topic_name); - } - } - if (link.GetTable("relay_both", &relay_both)) { - for (int j = 1; j <= relay_both.GetSize(); j++) { - // Agent topic configuration - relay_both.GetTable(j, &item_conf); - if (!item_conf.GetStr("name", &topic_name)) { - NODELET_ERROR("BridgeSubscriberNodelet: agent topic name not specified!"); - return; - } - sub.addTopic(topic_name, ns + topic_name); - } - } - } else if (link.GetStr("to", &config_agent) && config_agent == agent_name_) { - if (link.GetTable("relay_backward", &relay_backward)) { - for (int j = 1; j <= relay_backward.GetSize(); j++) { - // Agent topic configuration - relay_backward.GetTable(j, &item_conf); - if (!item_conf.GetStr("name", &topic_name)) { - NODELET_ERROR("BridgeSubscriberNodelet: agent topic name not specified!"); - return; - } - sub.addTopic(topic_name, ns + topic_name); - } - } - if (link.GetTable("relay_both", &relay_both)) { - for (int j = 1; j <= relay_both.GetSize(); j++) { - // Agent topic configuration - relay_both.GetTable(j, &item_conf); - if (!item_conf.GetStr("name", &topic_name)) { - NODELET_ERROR("BridgeSubscriberNodelet: agent topic name not specified!"); - return; - } - sub.addTopic(topic_name, ns + topic_name); - } - } - } - } - } - - private: - config_reader::ConfigReader config_params_; - std::string agent_name_; -}; - -PLUGINLIB_EXPORT_CLASS(comms_bridge::BridgeSubscriberNodelet, nodelet::Nodelet) - -} // namespace comms_bridge diff --git a/communications/comms_bridge/src/comms_bridge_nodelet.cpp b/communications/comms_bridge/src/comms_bridge_nodelet.cpp new file mode 100644 index 0000000000..dea0dce048 --- /dev/null +++ b/communications/comms_bridge/src/comms_bridge_nodelet.cpp @@ -0,0 +1,275 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +// Standard ROS includes +#include +#include +#include + +// FSW shared libraries +#include + +// FSW nodelet +#include +#include + +#include +#include + +#include +#include + +#include "comms_bridge/dds_ros_bridge_publisher.h" +#include "comms_bridge/generic_ros_sub_dds_pub.h" + +// SoraCore +#include "knDds/DdsSupport.h" +#include "knDds/DdsEntitiesFactory.h" +#include "knDds/DdsEntitiesFactorySvc.h" +#include "knDds/DdsTypedSupplier.h" + +// miro +#include "miro/Configuration.h" +#include "miro/Robot.h" +#include "miro/Log.h" + +namespace kn { + class DdsEntitiesFactorySvc; +} // end namespace kn + +namespace comms_bridge { + +class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { + public: + CommsBridgeNodelet() : ff_util::FreeFlyerNodelet("comms_bridge") {} + + virtual ~CommsBridgeNodelet() {} + + protected: + virtual void Initialize(ros::NodeHandle* nh) { + // Need to get robot name which is in the lua config files, add files, read + // files, and get robot name but don't get the rest of the config parameters + // since these params are used to create the classes that use rapid dds. Need + // to set up Miro/DDs before reading the parameters. + + config_params_.AddFile("communications/comms_bridge.config"); + + if (!config_params_.ReadFiles()) { + ROS_FATAL("BridgeSubscriberNodelet: Error reading config files."); + exit(EXIT_FAILURE); + return; + } + + if (!config_params_.GetStr("agent_name", &agent_name_)) { + ROS_FATAL("BridgeSubscriberNodelet: Could not read robot name."); + exit(EXIT_FAILURE); + return; + } + + // In simulation, the namespace is usually set to the robot name so we need to + // check if we are in simulation and get the right name + if (agent_name_ == "sim" || agent_name_ == "simulator") { + // The platform name should be the simulated robot name + agent_name_ = GetPlatform(); + + // If there is not robot name, set it to a default name so that we can + // connect to the bridge + if (agent_name_ == "") { + agent_name_ = "Bumble"; + } else { + // Make sure that first letter of robot name is capitialized. GDS only + // recognizes capitialized robot names. + agent_name_[0] = toupper(agent_name_[0]); + } + } + + int fake_argc = 1; + + // Make path to QOS and NDDS files + std::string config_path = ff_common::GetConfigDir(); + config_path += "/communications/dds_generic_comms/"; + + // Create fake argv containing only the participant name + // Participant name needs to be unique so combine robot name with timestamp + ros::Time time = ros::Time::now(); + participant_name_ = agent_name_ + std::to_string(time.sec) + std::string("-comms-bridge"); + char **fake_argv = new char*[1]; + fake_argv[0] = new char[(participant_name_.size() + 1)]; + std::strcpy(fake_argv[0], participant_name_.c_str()); // NOLINT + + /* fake miro log into thinking we have no arguments */ + Miro::Log::init(fake_argc, fake_argv); + Miro::Log::level(9); + + /* fake miro configuration into thinking we have no arguments */ + Miro::Configuration::init(fake_argc, fake_argv); + + Miro::RobotParameters *robot_params = Miro::RobotParameters::instance(); + + kn::DdsEntitiesFactorySvcParameters *dds_params = + kn::DdsEntitiesFactorySvcParameters::instance(); + + /* get the defaults for *all the things!* */ + Miro::ConfigDocument *config = Miro::Configuration::document(); + config->setSection("Robot"); + config->getParameters("Miro::RobotParameters", *robot_params); + config->getParameters("kn::DdsEntitiesFactorySvcParameters", *dds_params); + + robot_params->name = agent_name_; + robot_params->namingContextName = robot_params->name; + + // Set values for default punlisher and subscriber + dds_params->publishers[0].name = agent_name_; + dds_params->publishers[0].partition = agent_name_; + dds_params->publishers[0].participant = participant_name_; + dds_params->subscribers[0].participant = participant_name_; + + // Clear config files so that dds only looks for the files we add + dds_params->participants[0].discoveryPeersFiles.clear(); + dds_params->configFiles.clear(); + + dds_params->participants[0].name = participant_name_; + dds_params->participants[0].participantName = participant_name_; + dds_params->participants[0].domainId = 38; + dds_params->participants[0].discoveryPeersFiles.push_back( + (config_path + "NDDS_DISCOVERY_PEERS")); + dds_params->configFiles.push_back((config_path + "RAPID_QOS_PROFILES.xml")); + + std::string local_subscriber = Miro::RobotParameters::instance()->name.c_str(); + + if (!ReadParams()) { + exit(EXIT_FAILURE); + return; + } + + // Register the connections into the parameters so they can be used later + for (int i = 0; i <= rapid_connections_.size(); i++) { + // This shouldn't be needed but check just in case + if (local_subscriber != rapid_connections_[i]) { + kn::DdsNodeParameters subscriber; + subscriber.name = rapid_connections_[i]; + subscriber.partition = rapid_connections_[i]; + subscriber.participant = participant_name_; + dds_params->subscribers.push_back(subscriber); + } + } + + /** + * Use DdsEntitiesFactorySvc to create a new DdsEntitiesFactory + * which will create all objects: + * Participants DdsDomainParticipantRepository::instance() + * Publishers DdsPublisherRespository::instance() + * Subscribers DdsSubscriberRepository::instance() + * Topics + * and store in relevant repository + * based on DdsEntitiesFactoryParameters + */ + dds_entities_factory_.reset(new kn::DdsEntitiesFactorySvc()); + dds_entities_factory_->init(dds_params); + + ros_subs_.InitializeDDS(agent_name_); + } + + bool ReadParams() { + unsigned int verbose = 2; + if (!config_params_.GetUInt("verbose", &verbose)) { + NODELET_ERROR("Comms Bridge Nodelet: Could not read verbosity level. Setting to 2 (info?)."); + } + ros_subs_.setVerbosity(verbose); + // TODO(Katie): Fix this when you work on the publisher + // pubs.setVerbosity(verbose); + + std::string ns = std::string("/") + agent_name_ + "/"; + ns[1] = std::tolower(ns[1]); // namespaces don't start with upper case + + // Load shared topic groups + config_reader::ConfigReader::Table links, link; + if (!config_params_.GetTable("links", &links)) { + ROS_FATAL("Comms Bridge Nodelet: Links not specified!"); + return false; + } + + for (int i = 1; i <= links.GetSize(); i++) { + if (!links.GetTable(i, &link)) { + NODELET_ERROR("Comms Bridge Nodelet: Could read link table row %i", i); + continue; + } + std::string config_agent; + if (link.GetStr("from", &config_agent) && config_agent == agent_name_) { + AddRapidConnections(link, "to"); + AddTableToSubs(link, "relay_forward", ns); + AddTableToSubs(link, "relay_both", ns); + } else if (link.GetStr("to", &config_agent) && config_agent == agent_name_) { + AddRapidConnections(link, "from"); + AddTableToSubs(link, "relay_backward", ns); + AddTableToSubs(link, "relay_both", ns); + } + } + } + + void AddRapidConnections(config_reader::ConfigReader::Table &link_table, + std::string direction) { + std::string connection; + if (!link_table.GetStr(direction.c_str(), &connection)) { + NODELET_ERROR("Comms Bridge Nodelet: %s not specified for one link", direction); + return; + } + + // This should be very quick since we shouldn't have more than 2 connections + bool found = false; + for (int i = 0; i < rapid_connections_.size() && !found; i++) { + if (connection == rapid_connections_[i]) { + found = true; + } + } + + if (!found) { + rapid_connections_.push_back(connection); + } + } + + void AddTableToSubs(config_reader::ConfigReader::Table &link_table, + std::string table_name, + std::string ns) { + config_reader::ConfigReader::Table relay_table, relay_item; + std::string topic_name; + if (link_table.GetTable(table_name.c_str(), &relay_table)) { + for (int i = 1; i <= relay_table.GetSize(); i++) { + relay_table.GetTable(i, &relay_item); + if (!relay_item.GetStr("name", &topic_name)) { + NODELET_ERROR("Comms Bridge Nodelet: Agent topic name not specified!"); + continue; + } + ros_subs_.addTopic(topic_name, (ns + topic_name)); + } + } + } + + private: + config_reader::ConfigReader config_params_; + GenericROSSubDDSPub ros_subs_; + // TODO(Katie): Fix this when you work on the publisher + // DDSROSBridgePublisher ros_pubs_; + std::shared_ptr dds_entities_factory_; + std::string agent_name_, participant_name_; + std::vector rapid_connections_; +}; + +PLUGINLIB_EXPORT_CLASS(comms_bridge::CommsBridgeNodelet, nodelet::Nodelet) + +} // namespace comms_bridge diff --git a/communications/comms_bridge/src/generic_ros_sub_dds_pub.cpp b/communications/comms_bridge/src/generic_ros_sub_dds_pub.cpp new file mode 100644 index 0000000000..94a082ae59 --- /dev/null +++ b/communications/comms_bridge/src/generic_ros_sub_dds_pub.cpp @@ -0,0 +1,104 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "comms_bridge/generic_ros_sub_dds_pub.h" + +#include + +GenericROSSubDDSPub::GenericROSSubDDSPub() : + dds_initialized_(false), advertisement_info_seq_(0) {} + +GenericROSSubDDSPub::~GenericROSSubDDSPub() {} + +void GenericROSSubDDSPub::InitializeDDS(std::string agent_name) { + advertisement_info_supplier_.reset( + new GenericROSSubDDSPub::AdvertisementInfoSupplier( + rapid::ext::astrobee::GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC + agent_name, + "", "AstrobeeGenericCommsAdvertisementInfoProfile", "")); + + content_supplier_.reset(new GenericROSSubDDSPub::ContentSupplier( + rapid::ext::astrobee::GENERIC_COMMS_CONTENT_TOPIC + agent_name, + "", "AstrobeeGenericCommsContentProfile", "")); + + rapid::RapidHelper::initHeader(advertisement_info_supplier_->event().hdr); + rapid::RapidHelper::initHeader(content_supplier_->event().hdr); + + dds_initialized_ = true; +} + +// Called with the mutex held +void GenericROSSubDDSPub::subscribeTopic(std::string const& in_topic, const RelayTopicInfo& info) { + // this is just the base subscriber letting us know it's adding a topic + // nothing more we need to do +} + +// Called with the mutex held +void GenericROSSubDDSPub::advertiseTopic(const RelayTopicInfo& relay_info) { + const AdvertisementInfo &info = relay_info.ad_info; + rapid::ext::astrobee::GenericCommsAdvertisementInfo &msg = + advertisement_info_supplier_->event(); + unsigned int size; + std::string out_topic = relay_info.out_topic; + + msg.hdr.timeStamp = comms_util::RosTime2RapidTime(ros::Time::now()); + msg.hdr.serial = ++advertisement_info_seq_; + + // Currently the output topic can only be 128 characters long + SizeCheck(size, out_topic.size(), 128, "Out topic", out_topic); + std::strncpy(msg.outputTopic, out_topic.data(), size); + msg.outputTopic[size] = '\0'; + + msg.latching = info.latching; + + // Currently the data type can only be 128 characters long + SizeCheck(size, info.data_type.size(), 128, "Data type", out_topic); + std::strncpy(msg.dataType, info.data_type.data(), size); + msg.dataType[size] = '\0'; + + // Currently the md5 sum can only be 32 characters long + SizeCheck(size, info.md5_sum.size(), 32, "MD5 sum", out_topic); + std::strncpy(msg.md5Sum, info.md5_sum.data(), size); + msg.md5Sum[size] = '\0'; + + // Current the ROS message definition can only be 2048 characters long + SizeCheck(size, info.definition.size(), 2048, "Msg definition", out_topic); + std::strncpy(msg.msgDefinition, info.definition.data(), size); + msg.msgDefinition[size] = '\0'; + + // Send message + advertisement_info_supplier_->sendEvent(); +} + +// Called with the mutex held +void GenericROSSubDDSPub::relayMessage(const RelayTopicInfo& topic_info, ContentInfo const& content_info) { + // FIXME: DDS stuff here +} + +void GenericROSSubDDSPub::SizeCheck(unsigned int &size, + const int size_in, + const int max_size, + std::string const& data_name, + std::string const& topic) { + size = size_in; + // Account for null + if (size > (max_size - 1)) { + ROS_ERROR("Comms Bridge: %s for topic %s is greater than %i characters. Please change idl.", + data_name.c_str(), topic.c_str(), max_size); + size = (max_size - 1); + } +} diff --git a/communications/comms_bridge/src/ros_dds_bridge_subscriber.cpp b/communications/comms_bridge/src/ros_dds_bridge_subscriber.cpp deleted file mode 100644 index 874c7e6b6a..0000000000 --- a/communications/comms_bridge/src/ros_dds_bridge_subscriber.cpp +++ /dev/null @@ -1,111 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include "comms_bridge/ros_dds_bridge_subscriber.h" - -#include - -ROSDDSBridgeSubscriber::ROSDDSBridgeSubscriber(std::string agent_name) { - agent_name_ = agent_name; - - - int fake_argc = 1; - // Create fake argv containing only the participant name - // Participant name needs to uniue so combine robot name with timestamp - ros::Time time = ros::Time::now(); - participant_name_ = agent_name_ + std::to_string(time.sec) - + std::string("-multi-bridge"); - char **fake_argv = new char*[1]; - fake_argv[0] = new char[(participant_name_.size() + 1)]; - std::strcpy(fake_argv[0], participant_name_.c_str()); // NOLINT - - /* fake miro log into thinking we have no arguments */ - Miro::Log::init(fake_argc, fake_argv); - Miro::Log::level(9); - - /* fake miro configuration into thinking we have no arguments */ - Miro::Configuration::init(fake_argc, fake_argv); - - // Miro::RobotParameters *robot_params = Miro::RobotParameters::instance(); - // kn::DdsEntitiesFactorySvcParameters *dds_params = - // kn::DdsEntitiesFactorySvcParameters::instance(); - - // /* get the defaults for *all the things!* */ - // Miro::ConfigDocument *config = Miro::Configuration::document(); - // config->setSection("Robot"); - // config->getParameters("Miro::RobotParameters", *robot_params); - // config->getParameters("kn::DdsEntitiesFactorySvcParameters", *dds_params); - - // robot_params->name = agent_name_; - // robot_params->namingContextName = robot_params->name; - - // // Set values for default publisher and susbcriber - // dds_params->publishers[0].name = agent_name_; - // dds_params->publishers[0].partition = agent_name_; - // dds_params->publishers[0].participant = participant_name_; - // dds_params->subscribers[0].participant = participant_name_; - - // // Clear config files so that dds only looks for the files we add - // dds_params->participants[0].discoveryPeersFiles.clear(); - // dds_params->configFiles.clear(); - - // dds_params->participants[0].name = participant_name_; - // dds_params->participants[0].participantName = participant_name_; - // dds_params->participants[0].domainId = 38; - // dds_params->participants[0].discoveryPeersFiles.push_back( - // (config_path + "NDDS_DISCOVERY_PEERS")); - // dds_params->configFiles.push_back((config_path + "RAPID_QOS_PROFILES.xml")); - - - // /** - // * Use DdsEntitiesFactorySvc to create a new DdsEntitiesFactory - // * which will create all objects: - // * Participants DdsDomainParticpantRepository::instance() - // * Publishers DdsPublisherRespoitory::instance() - // * Subscribers DdsSubscriberRepository::instance() - // * Topics - // * and store in relevant repository - // * based on DdsEntitesFactoryParameters - // */ - // dds_entities_factory_.reset(new kn::DdsEntitiesFactorySvc()); - // dds_entities_factory_->init(dds_params); - - // trigger_srv_ = nh->advertiseService( - // SERVICE_COMMUNICATIONS_ENABLE_BRIDGE_SUBSCRIBER, - // &AstrobeeAstrobeeBridge::Start, - // this); -} - -ROSDDSBridgeSubscriber::~ROSDDSBridgeSubscriber() { - // FIXME: any needed cleanup -} - -// Called with the mutex held -void ROSDDSBridgeSubscriber::subscribeTopic(std::string const& in_topic, const RelayTopicInfo& info) { - // FIXME: DDS stuff here -} - -// Called with the mutex held -void ROSDDSBridgeSubscriber::advertiseTopic(const RelayTopicInfo& info) { - // FIXME: DDS stuff here -} - -// Called with the mutex held -void ROSDDSBridgeSubscriber::relayMessage(const RelayTopicInfo& topic_info, ContentInfo const& content_info) { - // FIXME: DDS stuff here -} diff --git a/communications/comms_bridge/src/util.cc b/communications/comms_bridge/src/util.cc new file mode 100644 index 0000000000..f2bbf9a682 --- /dev/null +++ b/communications/comms_bridge/src/util.cc @@ -0,0 +1,51 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include + +ros::Time comms_util::RapidTime2RosTime(const int64_t dds_time) { + ros::Time t; + + // DDS time is in microseconds, we need nanoseconds + t.fromNSec(static_cast(dds_time) * 1000); + + return t; +} + +int64_t comms_util::RosTime2RapidTime(ros::Time const& ros_time) { + return static_cast(ros_time.toNSec() / 1000ULL); +} + +void comms_util::RosHeader2Rapid(std_msgs::Header const& ros_hdr, + rapid::Header* rapid_hdr, + int status, + int serial) { + rapid::RapidHelper::initHeader(*rapid_hdr); + if (serial >= 0) { + rapid_hdr->serial = serial; + } + rapid_hdr->statusCode = status; + rapid_hdr->timeStamp = util::RosTime2RapidTime(ros_hdr.stamp); +} + +void comms_util::RapidHeader2Ros(rapid::Header const& rapid_hdr, + std_msgs::Header *ros_hdr, + std::string const& frame_id) { + ros_hdr->frame_id = frame_id; + ros_hdr->stamp = util::RapidTime2RosTime(rapid_hdr.timeStamp); +} diff --git a/communications/dds_msgs/idl/AstrobeeConstants.idl b/communications/dds_msgs/idl/AstrobeeConstants.idl index cbe7caaed9..8e8cd972f1 100644 --- a/communications/dds_msgs/idl/AstrobeeConstants.idl +++ b/communications/dds_msgs/idl/AstrobeeConstants.idl @@ -34,6 +34,9 @@ module rapid { const String64 EPS_STATE_TOPIC = "astrobee_eps_state"; const String64 FAULT_CONFIG_TOPIC = "astrobee_fault_config"; const String64 FAULT_STATE_TOPIC = "astrobee_fault_state"; + const String64 GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC = "astrobee_generic_comms_advertisement_info"; + const String64 GENERIC_COMMS_CONTENT_TOPIC = "astrobee_generic_comms_content"; + const String64 GENERIC_COMMS_RESET_TOPIC = "astrobee_generic_comms_reset"; const String64 GNC_CONTROL_STATE_TOPIC = "astrobee_gnc_control_state"; const String64 GNC_FAM_CMD_STATE_TOPIC = "astrobee_gnc_fam_cmd_state"; const String64 GUEST_SCIENCE_CONFIG_TOPIC = "astrobee_guest_science_config"; diff --git a/communications/dds_msgs/idl/GenericCommsAdvertisementInfo.idl b/communications/dds_msgs/idl/GenericCommsAdvertisementInfo.idl new file mode 100644 index 0000000000..0deeef6fcd --- /dev/null +++ b/communications/dds_msgs/idl/GenericCommsAdvertisementInfo.idl @@ -0,0 +1,45 @@ +/* + * Copyright 2023 (c) 2023 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-c-declaration class GenericCommsAdvertisementInfoTypeSupport; + //@copy-c-declaration class GenericCommsAdvertisementInfoDataWriter; + //@copy-c-declaration class GenericCommsAdvertisementInfoDataReader; + //@copy-c-declaration struct GenericCommsAdvertisementInfoSeq; + + //@copy-declaration /** + //@copy-declaration * Information of the topic type for generic comms. + //@copy-declaration */ + valuetype GenericCommsAdvertisementInfo : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef GenericCommsAdvertisementInfoTypeSupport TypeSupport; + //@copy-c-declaration typedef GenericCommsAdvertisementInfoDataWriter DataWriter; + //@copy-c-declaration typedef GenericCommsAdvertisementInfoDataReader DataReader; + //@copy-c-declaration typedef GenericCommsAdvertisementInfoSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef GenericCommsAdvertisementInfo Type; + + //@copy-declaration /** Topic on which to republish */ + public String128 outputTopic; + + //@copy-declaration /** Whether republisher should advertise topic as latching */ + public boolean latching; + + //@copy-declaration /** ROS message data type name */ + public String128 dataType; + + //@copy-declaration /** ROS message md5sum of type */ + public String32 md5Sum; + + //@copy-declaration /** ROS message definition */ + public String2K msgDefinition; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/GenericCommsContent.idl b/communications/dds_msgs/idl/GenericCommsContent.idl new file mode 100644 index 0000000000..5a44680192 --- /dev/null +++ b/communications/dds_msgs/idl/GenericCommsContent.idl @@ -0,0 +1,39 @@ +/* + * Copyright 2023 (c) 2023 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-c-declaration class GenericCommsContentTypeSupport; + //@copy-c-declaration class GenericCommsContentDataWriter; + //@copy-c-declaration class GenericCommsContentDataReader; + //@copy-c-declaration struct GenericCommsContentSeq; + + //@copy-declaration /** + //@copy-declaration * Content of a generic comms ROS message + //@copy-declaration */ + valuetype GenericCommsContent : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef GenericCommsContentTypeSupport TypeSupport; + //@copy-c-declaration typedef GenericCommsContentDataWriter DataWriter; + //@copy-c-declaration typedef GenericCommsContentDataReader DataReader; + //@copy-c-declaration typedef GenericCommsContentSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef GenericCommsContent Type; + + //@copy-declaration /** Topic on which to republish */ + public String128 outputTopic; + + //@copy-declaration /** The md5 sum of msg type repeated from the original advertisement */ + public String32 md5Sum; + + //@copy-declaration /** Serialized content of the message */ + public OctetSequence128K data; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/GenericCommsReset.idl b/communications/dds_msgs/idl/GenericCommsReset.idl new file mode 100644 index 0000000000..b295a5afff --- /dev/null +++ b/communications/dds_msgs/idl/GenericCommsReset.idl @@ -0,0 +1,33 @@ +/* + * Copyright 2023 (c) 2023 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + + //@copy-c-declaration class GenericCommsResetTypeSupport; + //@copy-c-declaration class GenericCommsResetDataWriter; + //@copy-c-declaration class GenericCommsResetDataReader; + //@copy-c-declaration struct GenericConmmsResetSeq; + + //@copy-declaration /** + //@copy-declaration * Reset message in case publisher didn't receive initial advertisement info message for this topic + //@copy-declaration */ + valuetype GenericCommsReset : Message { + //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) + //@copy-c-declaration typedef GenericCommsResetTypeSupport TypeSupport; + //@copy-c-declaration typedef GenericCommsResetDataWriter DataWriter; + //@copy-c-declaration typedef GenericCommsResetDataReader DataReader; + //@copy-c-declaration typedef GenericCommsResetSeq Seq; + //@copy-c-declaration #endif + //@copy-c-declaration typedef GenericCommsReset Type; + + //@copy-declaration // Topic for which metadata retransmission is requested + public String128 topic; + }; + }; + }; +}; diff --git a/communications/ff_msgs/msg/RelayAdvertisement.msg b/communications/ff_msgs/msg/GenericCommsAdvertisementInfo.msg similarity index 100% rename from communications/ff_msgs/msg/RelayAdvertisement.msg rename to communications/ff_msgs/msg/GenericCommsAdvertisementInfo.msg diff --git a/communications/ff_msgs/msg/RelayContent.msg b/communications/ff_msgs/msg/GenericCommsContent.msg similarity index 100% rename from communications/ff_msgs/msg/RelayContent.msg rename to communications/ff_msgs/msg/GenericCommsContent.msg diff --git a/communications/ff_msgs/msg/RelayReset.msg b/communications/ff_msgs/msg/GenericCommsReset.msg similarity index 100% rename from communications/ff_msgs/msg/RelayReset.msg rename to communications/ff_msgs/msg/GenericCommsReset.msg From daaaa89ce2a047dfc576132a8696177cc3374937 Mon Sep 17 00:00:00 2001 From: Katie Hamilton Date: Wed, 15 Nov 2023 16:25:58 -0800 Subject: [PATCH 10/32] Added advertisement info publisher. --- communications/comms_bridge/CMakeLists.txt | 8 +- .../comms_bridge/dds_ros_bridge_publisher.h | 46 ---------- .../comms_bridge/generic_rapid_msg_ros_pub.h | 47 +++++++++++ .../include/comms_bridge/generic_rapid_sub.h | 75 +++++++++++++++++ ..._dds_pub.h => generic_ros_sub_rapid_pub.h} | 16 ++-- .../comms_bridge/rapid_advertisement_info.h | 58 +++++++++++++ .../src/bridge_publisher_nodelet.cpp | 80 ------------------ .../comms_bridge/src/comms_bridge_nodelet.cpp | 28 ++++--- .../src/generic_rapid_msg_ros_pub.cpp | 84 +++++++++++++++++++ ...ge_publisher.cpp => generic_rapid_sub.cpp} | 34 +++++--- ..._pub.cpp => generic_ros_sub_rapid_pub.cpp} | 57 ++++++++++--- .../src/rapid_advertisement_info.cpp | 55 ++++++++++++ .../comms_bridge/src/{util.cc => util.cpp} | 11 +-- 13 files changed, 430 insertions(+), 169 deletions(-) delete mode 100644 communications/comms_bridge/include/comms_bridge/dds_ros_bridge_publisher.h create mode 100644 communications/comms_bridge/include/comms_bridge/generic_rapid_msg_ros_pub.h create mode 100644 communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h rename communications/comms_bridge/include/comms_bridge/{generic_ros_sub_dds_pub.h => generic_ros_sub_rapid_pub.h} (88%) create mode 100644 communications/comms_bridge/include/comms_bridge/rapid_advertisement_info.h delete mode 100644 communications/comms_bridge/src/bridge_publisher_nodelet.cpp create mode 100644 communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp rename communications/comms_bridge/src/{dds_ros_bridge_publisher.cpp => generic_rapid_sub.cpp} (50%) rename communications/comms_bridge/src/{generic_ros_sub_dds_pub.cpp => generic_ros_sub_rapid_pub.cpp} (63%) create mode 100644 communications/comms_bridge/src/rapid_advertisement_info.cpp rename communications/comms_bridge/src/{util.cc => util.cpp} (86%) diff --git a/communications/comms_bridge/CMakeLists.txt b/communications/comms_bridge/CMakeLists.txt index c60636a945..d4de1bfdce 100644 --- a/communications/comms_bridge/CMakeLists.txt +++ b/communications/comms_bridge/CMakeLists.txt @@ -95,9 +95,15 @@ include_directories( # Declare C++ libraries add_library(comms_bridge + src/bridge_publisher.cpp src/bridge_subscriber.cpp - src/generic_ros_sub_dds_pub.cpp + src/generic_rapid_msg_ros_pub.cpp + src/generic_ros_sub_rapid_pub.cpp src/comms_bridge_nodelet.cpp + src/generic_rapid_sub.cpp + src/rapid_advertisement_info.cpp + #src/rapid_content.cpp + src/util.cpp ) target_compile_definitions(comms_bridge PUBLIC ${RTIDDS_DEFINE_FLAGS}) add_dependencies(comms_bridge ${catkin_EXPORTED_TARGETS}) diff --git a/communications/comms_bridge/include/comms_bridge/dds_ros_bridge_publisher.h b/communications/comms_bridge/include/comms_bridge/dds_ros_bridge_publisher.h deleted file mode 100644 index cc01668c07..0000000000 --- a/communications/comms_bridge/include/comms_bridge/dds_ros_bridge_publisher.h +++ /dev/null @@ -1,46 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef COMMS_BRIDGE_DDS_ROS_BRIDGE_PUBLISHER_H_ -#define COMMS_BRIDGE_DDS_ROS_BRIDGE_PUBLISHER_H_ - -/* This is a specialization of ROSBridgePublisher using a DDS conduit -*/ - -#include -#include -#include - -#include - -// default time to delay between advertising and publishing on that topic [sec] -#define DEFAULT_DDSROSBRIDGE_PUB_ADVERTISE_DELAY 3.0 - -class DDSROSBridgePublisher : public BridgePublisher { - public: - // explicit DDSROSBridgePublisher(); - explicit DDSROSBridgePublisher(double ad2pub_delay = DEFAULT_DDSROSBRIDGE_PUB_ADVERTISE_DELAY); - virtual ~DDSROSBridgePublisher(); - - protected: - // prohibit shallow copy or assignment - DDSROSBridgePublisher(const DDSROSBridgePublisher&) : BridgePublisher(m_ad2pub_delay_) {} - void operator=(const DDSROSBridgePublisher&) {} -}; - -#endif // COMMS_BRIDGE_DDS_ROS_BRIDGE_PUBLISHER_H_ diff --git a/communications/comms_bridge/include/comms_bridge/generic_rapid_msg_ros_pub.h b/communications/comms_bridge/include/comms_bridge/generic_rapid_msg_ros_pub.h new file mode 100644 index 0000000000..11a9aff3e1 --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/generic_rapid_msg_ros_pub.h @@ -0,0 +1,47 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_GENERIC_RAPID_MSG_ROS_PUB_H_ +#define COMMS_BRIDGE_GENERIC_RAPID_MSG_ROS_PUB_H_ + +#include +#include + +#include +#include + +#include "dds_msgs/GenericCommsAdvertisementInfoSupport.h" +#include "dds_msgs/GenericCommsContentSupport.h" + +// default time to delay between advertisement and publishing on that topic [sec] +#define DEFAULT_ADVERTISE_TO_PUB_DELAY 3.0 + +namespace ff { + +class GenericRapidMsgRosPub : public BridgePublisher { + public: + explicit GenericRapidMsgRosPub(double ad2pub_delay = DEFAULT_ADVERTISE_TO_PUB_DELAY); + virtual ~GenericRapidMsgRosPub(); + + void ConvertAdvertisementInfo(rapid::ext::astrobee::GenericCommsAdvertisementInfo const* data); + void ConvertContent(rapid::ext::astrobee::GenericCommsContent const* data); +}; + +} // end namespace ff + +#endif // COMMS_BRIDGE_GENERIC_RAPID_MSG_ROS_PUB_H_ diff --git a/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h b/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h new file mode 100644 index 0000000000..19033d1cb7 --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h @@ -0,0 +1,75 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_GENERIC_RAPID_SUB_H_ +#define COMMS_BRIDGE_GENERIC_RAPID_SUB_H_ + +#include + +#include +#include +#include +#include + +#include "ros/ros.h" + +#include "knDds/DdsEventLoop.h" + +#include "knShare/Time.h" + +namespace ff { + +/** + * @brief base class for rapid subscriber to ros publisher + * @details base class for rapid subscriber to ros publisher. + * A kn::DdsEventLoop is run within its own thread of execution. + * Child classes must connect requeseted messege and callback + * to m_ddsEventLoop and call startThread() + */ +class GenericRapidSub { + protected: + GenericRapidSub(const std::string& entity_name, + const std::string& subscribe_topic, + GenericRapidMsgRosPub* rapid_msg_ros_pub); + ~GenericRapidSub(); + + /** + * Will start thread execution by calling threadExec() + */ + virtual void StartThread(); + + GenericRapidMsgRosPub* ros_pub_; + std::string subscribe_topic_; + + std::atomic alive_; + std::thread thread_; + kn::DdsEventLoop dds_event_loop_; + + private: + /** + * Function to execute within seperate thread + * process DdsEventLoop at 10Hz + */ + virtual void ThreadExec(); +}; + +typedef std::shared_ptr GenericRapidSubPtr; + +} // end namespace ff + +#endif // COMMS_BRIDGE_GENERIC_RAPID_SUB_H_ diff --git a/communications/comms_bridge/include/comms_bridge/generic_ros_sub_dds_pub.h b/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h similarity index 88% rename from communications/comms_bridge/include/comms_bridge/generic_ros_sub_dds_pub.h rename to communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h index 0ec7817e7c..3b3f559fd5 100644 --- a/communications/comms_bridge/include/comms_bridge/generic_ros_sub_dds_pub.h +++ b/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h @@ -16,8 +16,8 @@ * under the License. */ -#ifndef COMMS_BRIDGE_GENERIC_ROS_SUB_DDS_PUB_H_ -#define COMMS_BRIDGE_GENERIC_ROS_SUB_DDS_PUB_H_ +#ifndef COMMS_BRIDGE_GENERIC_ROS_SUB_RAPID_PUB_H_ +#define COMMS_BRIDGE_GENERIC_ROS_SUB_RAPID_PUB_H_ /* This is a specialization of ROSBridgeSubscriber using a DDS conduit */ @@ -39,10 +39,12 @@ #include "dds_msgs/GenericCommsAdvertisementInfoSupport.h" #include "dds_msgs/GenericCommsContentSupport.h" -class GenericROSSubDDSPub : public BridgeSubscriber { +namespace ff { + +class GenericROSSubRapidPub : public BridgeSubscriber { public: - GenericROSSubDDSPub(); - ~GenericROSSubDDSPub(); + GenericROSSubRapidPub(); + ~GenericROSSubRapidPub(); void InitializeDDS(std::string agent_name); void SizeCheck(unsigned int &size, @@ -78,4 +80,6 @@ class GenericROSSubDDSPub : public BridgeSubscriber { unsigned int advertisement_info_seq_; }; -#endif // COMMS_BRIDGE_GENERIC_ROS_SUB_DDS_PUB_H_ +} // end namespace ff + +#endif // COMMS_BRIDGE_GENERIC_ROS_SUB_RAPID_PUB_H_ diff --git a/communications/comms_bridge/include/comms_bridge/rapid_advertisement_info.h b/communications/comms_bridge/include/comms_bridge/rapid_advertisement_info.h new file mode 100644 index 0000000000..059cebd28d --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/rapid_advertisement_info.h @@ -0,0 +1,58 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_RAPID_ADVERTISEMENT_INFO_H_ +#define COMMS_BRIDGE_RAPID_ADVERTISEMENT_INFO_H_ + +#include +#include +#include + +#include "comms_bridge/generic_rapid_msg_ros_pub.h" +#include "comms_bridge/generic_rapid_sub.h" +#include "comms_bridge/util.h" + +#include "dds_msgs/AstrobeeConstants.h" +#include "dds_msgs/GenericCommsAdvertisementInfoSupport.h" + +#include "knDds/DdsTypedSupplier.h" + +#include "rapidDds/RapidConstants.h" + +#include "rapidUtil/RapidHelper.h" + +namespace ff { + +class RapidAdvertisementInfo : public GenericRapidSub { + public: + RapidAdvertisementInfo(const std::string& subscribe_topic, + const std::string& subscriber_partition, + GenericRapidMsgRosPub* rapid_msg_ros_pub); + + /** + * call back for ddsEventLoop + */ + void operator() (rapid::ext::astrobee::GenericCommsAdvertisementInfo const* data); + + private: + std::string subscriber_partition_; +}; + +} // end namespace ff + +#endif // COMMS_BRIDGE_RAPID_ADVERTISEMENT_INFO_H_ diff --git a/communications/comms_bridge/src/bridge_publisher_nodelet.cpp b/communications/comms_bridge/src/bridge_publisher_nodelet.cpp deleted file mode 100644 index ccc44da582..0000000000 --- a/communications/comms_bridge/src/bridge_publisher_nodelet.cpp +++ /dev/null @@ -1,80 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -// Standard ROS includes -#include -#include -#include - -// FSW shared libraries -#include - -// FSW nodelet -#include -#include - -#include -#include - -#include "comms_bridge/dds_ros_bridge_publisher.h" - - -namespace comms_bridge { - -class BridgePublisherNodelet : public ff_util::FreeFlyerNodelet { - public: - BridgePublisherNodelet() : ff_util::FreeFlyerNodelet("comms_bridge_pub") {} - - virtual ~BridgePublisherNodelet() {} - - protected: - virtual void Initialize(ros::NodeHandle* nh) { - config_params_.AddFile("communications/comms_bridge.config"); - - if (!config_params_.ReadFiles()) { - ROS_FATAL("BridgePublisherNodelet: Error reading config files."); - exit(EXIT_FAILURE); - return; - } - - unsigned int verbose; - if (!config_params_.GetUInt("verbose", &verbose)) { - ROS_FATAL("BridgePublisherNodelet: Could not read verbosity level."); - exit(EXIT_FAILURE); - return; - } - - double ad2pub_delay = 3.0; - if (!config_params_.GetReal("ad2pub_delay", &ad2pub_delay)) { - ROS_FATAL("BridgePublisherNodelet: Could not read advertiser to publisher delay."); - exit(EXIT_FAILURE); - return; - } - - DDSROSBridgePublisher pub(ad2pub_delay); - // if (verbose > 0) - // pub.setVerbosity(verbose); - } - - private: - config_reader::ConfigReader config_params_; -}; - -PLUGINLIB_EXPORT_CLASS(comms_bridge::BridgePublisherNodelet, nodelet::Nodelet) - -} // namespace comms_bridge diff --git a/communications/comms_bridge/src/comms_bridge_nodelet.cpp b/communications/comms_bridge/src/comms_bridge_nodelet.cpp index dea0dce048..8684ba8a86 100644 --- a/communications/comms_bridge/src/comms_bridge_nodelet.cpp +++ b/communications/comms_bridge/src/comms_bridge_nodelet.cpp @@ -34,8 +34,8 @@ #include #include -#include "comms_bridge/dds_ros_bridge_publisher.h" -#include "comms_bridge/generic_ros_sub_dds_pub.h" +#include "comms_bridge/generic_rapid_msg_ros_pub.h" +#include "comms_bridge/generic_ros_sub_rapid_pub.h" // SoraCore #include "knDds/DdsSupport.h" @@ -182,17 +182,26 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { dds_entities_factory_.reset(new kn::DdsEntitiesFactorySvc()); dds_entities_factory_->init(dds_params); - ros_subs_.InitializeDDS(agent_name_); + ros_sub_.InitializeDDS(agent_name_); + + // TODO(Katie): Add more publisher stuff here } bool ReadParams() { + double ad2pub_delay = 0; + if (!config_params_.GetReal("ad2pub_delay", &ad2pub_delay) || + ad2pub_delay <= 0) { + NODELET_ERROR("Comms Bridge Nodelet: Could not read/or invalid ad2pub_delay. Setting to 3."); + ad2pub_delay = 3; + } + ros_pub_ = std::make_shared(ad2pub_delay); + unsigned int verbose = 2; if (!config_params_.GetUInt("verbose", &verbose)) { NODELET_ERROR("Comms Bridge Nodelet: Could not read verbosity level. Setting to 2 (info?)."); } - ros_subs_.setVerbosity(verbose); - // TODO(Katie): Fix this when you work on the publisher - // pubs.setVerbosity(verbose); + ros_sub_.setVerbosity(verbose); + ros_pub_->setVerbosity(verbose); std::string ns = std::string("/") + agent_name_ + "/"; ns[1] = std::tolower(ns[1]); // namespaces don't start with upper case @@ -255,17 +264,16 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { NODELET_ERROR("Comms Bridge Nodelet: Agent topic name not specified!"); continue; } - ros_subs_.addTopic(topic_name, (ns + topic_name)); + ros_sub_.addTopic(topic_name, (ns + topic_name)); } } } private: config_reader::ConfigReader config_params_; - GenericROSSubDDSPub ros_subs_; - // TODO(Katie): Fix this when you work on the publisher - // DDSROSBridgePublisher ros_pubs_; + ff::GenericROSSubRapidPub ros_sub_; std::shared_ptr dds_entities_factory_; + std::shared_ptr ros_pub_; std::string agent_name_, participant_name_; std::vector rapid_connections_; }; diff --git a/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp new file mode 100644 index 0000000000..27f0d886bd --- /dev/null +++ b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp @@ -0,0 +1,84 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include + +#include +#include + +namespace ff { + +GenericRapidMsgRosPub::GenericRapidMsgRosPub(double ad2pub_delay) : + BridgePublisher(ad2pub_delay) { +} + +GenericRapidMsgRosPub::~GenericRapidMsgRosPub() {} + +void GenericRapidMsgRosPub::ConvertAdvertisementInfo( + rapid::ext::astrobee::GenericCommsAdvertisementInfo const* data) { + const std::lock_guard lock(m_mutex_); + + const std::string output_topic = data->outputTopic; + + ROS_DEBUG("Comms Bridge Nodelet: Received advertisement message for topic %s\n", + output_topic.c_str()); + + AdvertisementInfo ad_info; + ad_info.latching = data->latching; + ad_info.data_type = data->dataType; + ad_info.md5_sum = data->md5Sum; + ad_info.definition = data->msgDefinition; + + if (!advertiseTopic(output_topic, ad_info)) { + ROS_ERROR("Comms Bridge Nodelet: Error advertising topic: %s\n", + output_topic.c_str()); + } +} + +void GenericRapidMsgRosPub::ConvertContent( + rapid::ext::astrobee::GenericCommsContent const* data) { + const std::lock_guard lock(m_mutex_); + + const std::string output_topic = data->outputTopic; + + ROS_DEBUG("Comms Bridge Nodelet: Received content message for topic %s\n", + output_topic.c_str()); + + std::map::iterator iter = m_relay_topics_.find(output_topic); + if (iter == m_relay_topics_.end()) { + ROS_ERROR("Comms Bridge Nodelet: Received content for topic %s but never received advertisement info.\n", + output_topic); + } else { + RelayTopicInfo &topic_info = iter->second; + + ContentInfo content_info; + content_info.type_md5_sum = data->md5Sum; + + unsigned char* buf = data->data.get_contiguous_buffer(); + for (size_t i = 0; i < data->data.length(); i++) { + content_info.data.push_back(buf[i]); + } + + if (!relayMessage(topic_info, content_info)) { + ROS_ERROR("Comms Bridge Nodelet: Error relaying message for topic %s\n", + output_topic.c_str()); + } + } +} + +} // end namespace ff diff --git a/communications/comms_bridge/src/dds_ros_bridge_publisher.cpp b/communications/comms_bridge/src/generic_rapid_sub.cpp similarity index 50% rename from communications/comms_bridge/src/dds_ros_bridge_publisher.cpp rename to communications/comms_bridge/src/generic_rapid_sub.cpp index da1d525512..61f1889c65 100644 --- a/communications/comms_bridge/src/dds_ros_bridge_publisher.cpp +++ b/communications/comms_bridge/src/generic_rapid_sub.cpp @@ -16,17 +16,31 @@ * under the License. */ -#include "comms_bridge/dds_ros_bridge_publisher.h" +#include "comms_bridge/generic_rapid_sub.h" -DDSROSBridgePublisher::DDSROSBridgePublisher(double ad2pub_delay) : BridgePublisher(ad2pub_delay) {} +#include -DDSROSBridgePublisher::~DDSROSBridgePublisher() {} +namespace ff { -// When the peered subscriber transmits advertisement information, call: -// advertiseTopic(const std::string &output_topic, const AdvertisementInfo &ad_info) -// while holding m_mutex -// to setup the output ROS advertisement +GenericRapidSub::GenericRapidSub(const std::string& entity_name, const std::string& subscribe_topic, + GenericRapidMsgRosPub* rapid_msg_ros_pub) + : dds_event_loop_(entity_name), subscribe_topic_(subscribe_topic), ros_pub_(rapid_msg_ros_pub) {} -// When the peered subscriber transmits a serialized message, call: -// relayMessage(RelayTopicInfo &topic_info, const ContentInfo &content_info) -// to publish the output ROS message +GenericRapidSub::~GenericRapidSub() { + alive_ = false; // Notify thread to exit + thread_.join(); +} + +void GenericRapidSub::StartThread() { + // start joinable thread + thread_ = std::thread(&GenericRapidSub::ThreadExec, this); +} + +void GenericRapidSub::ThreadExec() { + while (alive_) { + // process events at 10hz + dds_event_loop_.processEvents(kn::milliseconds(100)); + } +} + +} // end namespace ff diff --git a/communications/comms_bridge/src/generic_ros_sub_dds_pub.cpp b/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp similarity index 63% rename from communications/comms_bridge/src/generic_ros_sub_dds_pub.cpp rename to communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp index 94a082ae59..4d1666effd 100644 --- a/communications/comms_bridge/src/generic_ros_sub_dds_pub.cpp +++ b/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp @@ -16,22 +16,24 @@ * under the License. */ -#include "comms_bridge/generic_ros_sub_dds_pub.h" +#include "comms_bridge/generic_ros_sub_rapid_pub.h" #include -GenericROSSubDDSPub::GenericROSSubDDSPub() : +namespace ff { + +GenericROSSubRapidPub::GenericROSSubRapidPub() : dds_initialized_(false), advertisement_info_seq_(0) {} -GenericROSSubDDSPub::~GenericROSSubDDSPub() {} +GenericROSSubRapidPub::~GenericROSSubRapidPub() {} -void GenericROSSubDDSPub::InitializeDDS(std::string agent_name) { +void GenericROSSubRapidPub::InitializeDDS(std::string agent_name) { advertisement_info_supplier_.reset( - new GenericROSSubDDSPub::AdvertisementInfoSupplier( + new GenericROSSubRapidPub::AdvertisementInfoSupplier( rapid::ext::astrobee::GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC + agent_name, "", "AstrobeeGenericCommsAdvertisementInfoProfile", "")); - content_supplier_.reset(new GenericROSSubDDSPub::ContentSupplier( + content_supplier_.reset(new GenericROSSubRapidPub::ContentSupplier( rapid::ext::astrobee::GENERIC_COMMS_CONTENT_TOPIC + agent_name, "", "AstrobeeGenericCommsContentProfile", "")); @@ -42,13 +44,13 @@ void GenericROSSubDDSPub::InitializeDDS(std::string agent_name) { } // Called with the mutex held -void GenericROSSubDDSPub::subscribeTopic(std::string const& in_topic, const RelayTopicInfo& info) { +void GenericROSSubRapidPub::subscribeTopic(std::string const& in_topic, const RelayTopicInfo& info) { // this is just the base subscriber letting us know it's adding a topic // nothing more we need to do } // Called with the mutex held -void GenericROSSubDDSPub::advertiseTopic(const RelayTopicInfo& relay_info) { +void GenericROSSubRapidPub::advertiseTopic(const RelayTopicInfo& relay_info) { const AdvertisementInfo &info = relay_info.ad_info; rapid::ext::astrobee::GenericCommsAdvertisementInfo &msg = advertisement_info_supplier_->event(); @@ -85,11 +87,42 @@ void GenericROSSubDDSPub::advertiseTopic(const RelayTopicInfo& relay_info) { } // Called with the mutex held -void GenericROSSubDDSPub::relayMessage(const RelayTopicInfo& topic_info, ContentInfo const& content_info) { - // FIXME: DDS stuff here +void GenericROSSubRapidPub::relayMessage(const RelayTopicInfo& topic_info, ContentInfo const& content_info) { + rapid::ext::astrobee::GenericCommsContent &msg = content_supplier_->event(); + + unsigned int size; + std::string out_topic = topic_info.out_topic; + + msg.hdr.timeStamp = comms_util::RosTime2RapidTime(ros::Time::now()); + msg.hdr.serial = topic_info.relay_seqnum; + + // Currently the output topic can only be 128 characters long + SizeCheck(size, out_topic.size(), 128, "Out topic", out_topic); + std::strncpy(msg.outputTopic, out_topic.data(), size); + msg.outputTopic[size] = '\0'; + + // Currently the md5 sum can only be 32 characters long + SizeCheck(size, content_info.type_md5_sum.size(), 32, "MD5 sum", out_topic); + std::strncpy(msg.md5Sum, content_info.type_md5_sum.data(), size); + msg.md5Sum[size] = '\0'; + + // Currently the content can only be 128K bytes long + SizeCheck(size, content_info.data_size, 131072, "Data", out_topic); + msg.data.ensure_length(size, (size + 1)); + unsigned char *buf = msg.data.get_contiguous_buffer(); + if (buf == NULL) { + ROS_ERROR("DDS: RTI didn't give a contiguous buffer for the content data!"); + return; + } + + std::memset(buf, 0, (size + 1)); + std::memmove(buf, content_info.data, size); + + // Send message + content_supplier_->sendEvent(); } -void GenericROSSubDDSPub::SizeCheck(unsigned int &size, +void GenericROSSubRapidPub::SizeCheck(unsigned int &size, const int size_in, const int max_size, std::string const& data_name, @@ -102,3 +135,5 @@ void GenericROSSubDDSPub::SizeCheck(unsigned int &size, size = (max_size - 1); } } + +} // end namespace ff diff --git a/communications/comms_bridge/src/rapid_advertisement_info.cpp b/communications/comms_bridge/src/rapid_advertisement_info.cpp new file mode 100644 index 0000000000..2d3a2a42f9 --- /dev/null +++ b/communications/comms_bridge/src/rapid_advertisement_info.cpp @@ -0,0 +1,55 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "comms_bridge/rapid_advertisement_info.h" + +#include + +namespace ff { + +RapidAdvertisementInfo::RapidAdvertisementInfo(const std::string& subscribe_topic, + const std::string& subscriber_partition, + GenericRapidMsgRosPub* rapid_msg_ros_pub) + : GenericRapidSub("AstrobeeGenericCommsAdvertisementInfoProfile", subscribe_topic, rapid_msg_ros_pub), + subscriber_partition_(subscriber_partition) { + // connect to ddsEventLoop + try { + dds_event_loop_.connect( + this, + subscribe_topic, // topic + subscriber_partition, // name + "AstrobeeGenericCommsAdvertisementInfo", // profile + ""); // library + } catch (std::exception& e) { + ROS_ERROR_STREAM("RapidAdvertisementInfo exception: " << e.what()); + throw; + } catch (...) { + ROS_ERROR("RapidAdvertisementInfo exception unknown"); + throw; + } + + // start thread + StartThread(); +} + +void RapidAdvertisementInfo::operator() ( + rapid::ext::astrobee::GenericCommsAdvertisementInfo const* data) { + ros_pub_->ConvertAdvertisementInfo(data); +} + +} // end namespace ff diff --git a/communications/comms_bridge/src/util.cc b/communications/comms_bridge/src/util.cpp similarity index 86% rename from communications/comms_bridge/src/util.cc rename to communications/comms_bridge/src/util.cpp index f2bbf9a682..d90f9bd01c 100644 --- a/communications/comms_bridge/src/util.cc +++ b/communications/comms_bridge/src/util.cpp @@ -18,6 +18,8 @@ #include +#include + ros::Time comms_util::RapidTime2RosTime(const int64_t dds_time) { ros::Time t; @@ -40,12 +42,11 @@ void comms_util::RosHeader2Rapid(std_msgs::Header const& ros_hdr, rapid_hdr->serial = serial; } rapid_hdr->statusCode = status; - rapid_hdr->timeStamp = util::RosTime2RapidTime(ros_hdr.stamp); + rapid_hdr->timeStamp = comms_util::RosTime2RapidTime(ros_hdr.stamp); } -void comms_util::RapidHeader2Ros(rapid::Header const& rapid_hdr, - std_msgs::Header *ros_hdr, - std::string const& frame_id) { +void comms_util::RapidHeader2Ros(rapid::Header const& rapid_hdr, std_msgs::Header* ros_hdr, + std::string const& frame_id) { ros_hdr->frame_id = frame_id; - ros_hdr->stamp = util::RapidTime2RosTime(rapid_hdr.timeStamp); + ros_hdr->stamp = comms_util::RapidTime2RosTime(rapid_hdr.timeStamp); } From db2035da3a2b9334290bb300af6f397ec4e697bf Mon Sep 17 00:00:00 2001 From: Katie Hamilton Date: Sun, 19 Nov 2023 20:26:51 -0800 Subject: [PATCH 11/32] Finished initial comms bridge code. --- communications/comms_bridge/CMakeLists.txt | 13 +- .../comms_bridge/generic_rapid_msg_ros_pub.h | 4 +- .../include/comms_bridge/generic_rapid_sub.h | 66 +++++++--- .../comms_bridge/generic_ros_sub_rapid_pub.h | 3 +- .../comms_bridge/rapid_advertisement_info.h | 58 -------- .../comms_bridge/ros_ros_bridge_publisher.h | 64 --------- .../comms_bridge/ros_ros_bridge_subscriber.h | 70 ---------- .../comms_bridge/src/comms_bridge_nodelet.cpp | 35 ++++- .../src/generic_rapid_msg_ros_pub.cpp | 4 +- .../comms_bridge/src/generic_rapid_sub.cpp | 46 ------- .../src/generic_ros_sub_rapid_pub.cpp | 6 +- .../src/rapid_advertisement_info.cpp | 55 -------- .../src/ros_ros_bridge_publisher.cpp | 121 ----------------- .../src/ros_ros_bridge_subscriber.cpp | 124 ------------------ .../dds_msgs/idl/AstrobeeConstants.idl | 1 - .../dds_msgs/idl/GenericCommsReset.idl | 33 ----- 16 files changed, 93 insertions(+), 610 deletions(-) delete mode 100644 communications/comms_bridge/include/comms_bridge/rapid_advertisement_info.h delete mode 100644 communications/comms_bridge/include/comms_bridge/ros_ros_bridge_publisher.h delete mode 100644 communications/comms_bridge/include/comms_bridge/ros_ros_bridge_subscriber.h delete mode 100644 communications/comms_bridge/src/generic_rapid_sub.cpp delete mode 100644 communications/comms_bridge/src/rapid_advertisement_info.cpp delete mode 100644 communications/comms_bridge/src/ros_ros_bridge_publisher.cpp delete mode 100644 communications/comms_bridge/src/ros_ros_bridge_subscriber.cpp delete mode 100644 communications/dds_msgs/idl/GenericCommsReset.idl diff --git a/communications/comms_bridge/CMakeLists.txt b/communications/comms_bridge/CMakeLists.txt index d4de1bfdce..a9f6aaf995 100644 --- a/communications/comms_bridge/CMakeLists.txt +++ b/communications/comms_bridge/CMakeLists.txt @@ -92,18 +92,13 @@ include_directories( ${Boost_INCLUDE_DIRS} ) +file(GLOB cc_files + "src/*.cpp" +) # Declare C++ libraries add_library(comms_bridge - src/bridge_publisher.cpp - src/bridge_subscriber.cpp - src/generic_rapid_msg_ros_pub.cpp - src/generic_ros_sub_rapid_pub.cpp - src/comms_bridge_nodelet.cpp - src/generic_rapid_sub.cpp - src/rapid_advertisement_info.cpp - #src/rapid_content.cpp - src/util.cpp + ${cc_files} ) target_compile_definitions(comms_bridge PUBLIC ${RTIDDS_DEFINE_FLAGS}) add_dependencies(comms_bridge ${catkin_EXPORTED_TARGETS}) diff --git a/communications/comms_bridge/include/comms_bridge/generic_rapid_msg_ros_pub.h b/communications/comms_bridge/include/comms_bridge/generic_rapid_msg_ros_pub.h index 11a9aff3e1..16e46f9c05 100644 --- a/communications/comms_bridge/include/comms_bridge/generic_rapid_msg_ros_pub.h +++ b/communications/comms_bridge/include/comms_bridge/generic_rapid_msg_ros_pub.h @@ -38,8 +38,8 @@ class GenericRapidMsgRosPub : public BridgePublisher { explicit GenericRapidMsgRosPub(double ad2pub_delay = DEFAULT_ADVERTISE_TO_PUB_DELAY); virtual ~GenericRapidMsgRosPub(); - void ConvertAdvertisementInfo(rapid::ext::astrobee::GenericCommsAdvertisementInfo const* data); - void ConvertContent(rapid::ext::astrobee::GenericCommsContent const* data); + void ConvertData(rapid::ext::astrobee::GenericCommsAdvertisementInfo const* data); + void ConvertData(rapid::ext::astrobee::GenericCommsContent const* data); }; } // end namespace ff diff --git a/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h b/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h index 19033d1cb7..fcd09522de 100644 --- a/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h +++ b/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h @@ -32,43 +32,75 @@ #include "knShare/Time.h" +#include "dds_msgs/GenericCommsAdvertisementInfoSupport.h" +#include "dds_msgs/GenericCommsContentSupport.h" + namespace ff { -/** - * @brief base class for rapid subscriber to ros publisher - * @details base class for rapid subscriber to ros publisher. - * A kn::DdsEventLoop is run within its own thread of execution. - * Child classes must connect requeseted messege and callback - * to m_ddsEventLoop and call startThread() - */ +template class GenericRapidSub { - protected: + public: GenericRapidSub(const std::string& entity_name, const std::string& subscribe_topic, - GenericRapidMsgRosPub* rapid_msg_ros_pub); - ~GenericRapidSub(); + const std::string& subscriber_partition, + GenericRapidMsgRosPub* rapid_msg_ros_pub) + : dds_event_loop_(entity_name), + subscribe_topic_(subscribe_topic), + subscriber_partition_(subscriber_partition), + ros_pub_(rapid_msg_ros_pub) { + // connect to ddsEventLoop + try { + dds_event_loop_.connect(this, + subscribe_topic, // topic + subscriber_partition, // name + entity_name, // profile + ""); // library + } catch (std::exception& e) { + ROS_ERROR_STREAM("Rapid exception: " << e.what()); + throw; + } catch (...) { + ROS_ERROR("Rapid exception unknown"); + throw; + } - /** - * Will start thread execution by calling threadExec() - */ - virtual void StartThread(); + // start joinable thread + thread_ = std::thread(&GenericRapidSub::ThreadExec, this); + } + + ~GenericRapidSub() { + alive_ = false; // Notify thread to exit + thread_.join(); + } + void operator() (T const* data) { + ros_pub_->ConvertData(data); + } + + private: GenericRapidMsgRosPub* ros_pub_; std::string subscribe_topic_; + std::string subscriber_partition_; std::atomic alive_; std::thread thread_; kn::DdsEventLoop dds_event_loop_; - private: /** * Function to execute within seperate thread * process DdsEventLoop at 10Hz */ - virtual void ThreadExec(); + void ThreadExec() { + while (alive_) { + // process events at 10hz + dds_event_loop_.processEvents(kn::milliseconds(100)); + } + } }; -typedef std::shared_ptr GenericRapidSubPtr; +typedef std::shared_ptr> + AdvertisementInfoRapidSubPtr; +typedef std::shared_ptr> + ContentRapidSubPtr; } // end namespace ff diff --git a/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h b/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h index 3b3f559fd5..42d8ee0205 100644 --- a/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h +++ b/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h @@ -46,7 +46,8 @@ class GenericROSSubRapidPub : public BridgeSubscriber { GenericROSSubRapidPub(); ~GenericROSSubRapidPub(); - void InitializeDDS(std::string agent_name); + void InitializeDDS(); + void SizeCheck(unsigned int &size, const int size_in, const int max_size, diff --git a/communications/comms_bridge/include/comms_bridge/rapid_advertisement_info.h b/communications/comms_bridge/include/comms_bridge/rapid_advertisement_info.h deleted file mode 100644 index 059cebd28d..0000000000 --- a/communications/comms_bridge/include/comms_bridge/rapid_advertisement_info.h +++ /dev/null @@ -1,58 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef COMMS_BRIDGE_RAPID_ADVERTISEMENT_INFO_H_ -#define COMMS_BRIDGE_RAPID_ADVERTISEMENT_INFO_H_ - -#include -#include -#include - -#include "comms_bridge/generic_rapid_msg_ros_pub.h" -#include "comms_bridge/generic_rapid_sub.h" -#include "comms_bridge/util.h" - -#include "dds_msgs/AstrobeeConstants.h" -#include "dds_msgs/GenericCommsAdvertisementInfoSupport.h" - -#include "knDds/DdsTypedSupplier.h" - -#include "rapidDds/RapidConstants.h" - -#include "rapidUtil/RapidHelper.h" - -namespace ff { - -class RapidAdvertisementInfo : public GenericRapidSub { - public: - RapidAdvertisementInfo(const std::string& subscribe_topic, - const std::string& subscriber_partition, - GenericRapidMsgRosPub* rapid_msg_ros_pub); - - /** - * call back for ddsEventLoop - */ - void operator() (rapid::ext::astrobee::GenericCommsAdvertisementInfo const* data); - - private: - std::string subscriber_partition_; -}; - -} // end namespace ff - -#endif // COMMS_BRIDGE_RAPID_ADVERTISEMENT_INFO_H_ diff --git a/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_publisher.h b/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_publisher.h deleted file mode 100644 index be1e5bca6a..0000000000 --- a/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_publisher.h +++ /dev/null @@ -1,64 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef COMMS_BRIDGE_ROS_ROS_BRIDGE_PUBLISHER_H_ -#define COMMS_BRIDGE_ROS_ROS_BRIDGE_PUBLISHER_H_ - -/* This is a specialization of ROSBridgePublisher using a ROS conduit - that is another topic on the same ROS master - ie, it's a ROS-ROS bridge over ROS -*/ - -#include -#include -#include - -#include - -#include - -#define DEFAULT_ROSROSBRIDGE_PUB_META_TOPIC_PREFIX "/polymorph_relay" -// default time to delay between advertising and publishing on that topic [sec] -#define DEFAULT_ROSROSBRIDGE_PUB_ADVERTISE_DELAY 3.0 - -class ROSROSBridgePublisher : public BridgePublisher { - public: - ROSROSBridgePublisher(const std::string& meta_topic_prefix = DEFAULT_ROSROSBRIDGE_PUB_META_TOPIC_PREFIX, - double ad2pub_delay = DEFAULT_ROSROSBRIDGE_PUB_ADVERTISE_DELAY); - virtual ~ROSROSBridgePublisher(); - - protected: - void setupMetaChannels(); - void setupReverseMetaChannels(); - - void requestTopicInfo(std::string const& output_topic); - - void handleContentMessage(const ff_msgs::RelayContent::ConstPtr& msg); - void handleAdMessage(const ff_msgs::RelayAdvertisement::ConstPtr& msg); - - std::string m_meta_topic_prefix; - ros::Subscriber m_advert_sub; - ros::Subscriber m_content_sub; - ros::Publisher m_reset_pub; - - // prohibit shallow copy or assignment - ROSROSBridgePublisher(const ROSROSBridgePublisher&) : BridgePublisher(m_ad2pub_delay) {} - void operator=(const ROSROSBridgePublisher&) {} -}; - -#endif // COMMS_BRIDGE_ROS_ROS_BRIDGE_PUBLISHER_H_ diff --git a/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_subscriber.h b/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_subscriber.h deleted file mode 100644 index c53650d2be..0000000000 --- a/communications/comms_bridge/include/comms_bridge/ros_ros_bridge_subscriber.h +++ /dev/null @@ -1,70 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#ifndef COMMS_BRIDGE_ROS_ROS_BRIDGE_SUBSCRIBER_H_ -#define COMMS_BRIDGE_ROS_ROS_BRIDGE_SUBSCRIBER_H_ - -/* This is a specialization of ROSBridgeSubscriber using a ROS conduit - that is another topic on the same ROS master - ie, it's a ROS-ROS bridge over ROS -*/ - -#include -#include -#include - -#include - -#include -#include - -#define DEFAULT_ROSROSBRIDGE_PUB_META_TOPIC_PREFIX "/polymorph_relay" - -class ROSROSBridgeSubscriber : public BridgeSubscriber { - public: - explicit ROSROSBridgeSubscriber(const std::string& meta_topic_prefix = DEFAULT_ROSROSBRIDGE_PUB_META_TOPIC_PREFIX); - virtual ~ROSROSBridgeSubscriber(); - - // Called with the mutex held - virtual void subscribeTopic(std::string const& in_topic, const RelayTopicInfo& info); - - // Called with the mutex held - virtual void advertiseTopic(const RelayTopicInfo& info); - - // Called with the mutex held - virtual void relayMessage(const RelayTopicInfo& topic_info, ContentInfo const& content_info); - - protected: - void setupMetaChannels(); - void setupReverseMetaChannels(); - void handleResetMessage(const ff_msgs::RelayReset::ConstPtr& msg); - - std::string m_meta_topic_prefix; - unsigned int m_n_advertised; - ros::Publisher m_advertiser_pub; - ros::Publisher m_relayer_pub; - ros::Subscriber m_reset_sub; - - std::map requested_resets; // topics requiring re-advert - - // prohibit shallow copy or assignment - ROSROSBridgeSubscriber(const ROSROSBridgeSubscriber&) {} - void operator=(const ROSROSBridgeSubscriber&) {} -}; - -#endif // COMMS_BRIDGE_ROS_ROS_BRIDGE_SUBSCRIBER_H_ diff --git a/communications/comms_bridge/src/comms_bridge_nodelet.cpp b/communications/comms_bridge/src/comms_bridge_nodelet.cpp index 8684ba8a86..dc3d462088 100644 --- a/communications/comms_bridge/src/comms_bridge_nodelet.cpp +++ b/communications/comms_bridge/src/comms_bridge_nodelet.cpp @@ -35,8 +35,11 @@ #include #include "comms_bridge/generic_rapid_msg_ros_pub.h" +#include "comms_bridge/generic_rapid_sub.h" #include "comms_bridge/generic_ros_sub_rapid_pub.h" +#include "dds_msgs/AstrobeeConstants.h" + // SoraCore #include "knDds/DdsSupport.h" #include "knDds/DdsEntitiesFactory.h" @@ -182,9 +185,31 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { dds_entities_factory_.reset(new kn::DdsEntitiesFactorySvc()); dds_entities_factory_->init(dds_params); - ros_sub_.InitializeDDS(agent_name_); + InitializeDDS(); + } - // TODO(Katie): Add more publisher stuff here + void InitializeDDS() { + std::string connection; + ff::AdvertisementInfoRapidSubPtr advertisement_info_sub; + ff::ContentRapidSubPtr content_sub; + ros_sub_.InitializeDDS(); + for (size_t i = 0; i < rapid_connections_.size(); i++) { + // Lower case the external agent name to use it like a namespace + connection = rapid_connections_[i]; + connection[0] = tolower(connection[0]); + advertisement_info_sub = + std::make_shared>( + "AstrobeeGenericCommsAdvertisementInfoProfile", rapid::ext::astrobee::GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC, + connection, ros_pub_.get()); + advertisement_info_rapid_subs_.push_back(advertisement_info_sub); + + content_sub = std::make_shared>( + "AstrobeeGenericCommsContentProfile", + rapid::ext::astrobee::GENERIC_COMMS_CONTENT_TOPIC, + connection, + ros_pub_.get()); + content_rapid_subs_.push_back(content_sub); + } } bool ReadParams() { @@ -241,7 +266,7 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { // This should be very quick since we shouldn't have more than 2 connections bool found = false; - for (int i = 0; i < rapid_connections_.size() && !found; i++) { + for (size_t i = 0; i < rapid_connections_.size() && !found; i++) { if (connection == rapid_connections_[i]) { found = true; } @@ -258,7 +283,7 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { config_reader::ConfigReader::Table relay_table, relay_item; std::string topic_name; if (link_table.GetTable(table_name.c_str(), &relay_table)) { - for (int i = 1; i <= relay_table.GetSize(); i++) { + for (size_t i = 1; i <= relay_table.GetSize(); i++) { relay_table.GetTable(i, &relay_item); if (!relay_item.GetStr("name", &topic_name)) { NODELET_ERROR("Comms Bridge Nodelet: Agent topic name not specified!"); @@ -272,6 +297,8 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { private: config_reader::ConfigReader config_params_; ff::GenericROSSubRapidPub ros_sub_; + std::vector content_rapid_subs_; + std::vector advertisement_info_rapid_subs_; std::shared_ptr dds_entities_factory_; std::shared_ptr ros_pub_; std::string agent_name_, participant_name_; diff --git a/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp index 27f0d886bd..f981dca3fc 100644 --- a/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp +++ b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp @@ -29,7 +29,7 @@ GenericRapidMsgRosPub::GenericRapidMsgRosPub(double ad2pub_delay) : GenericRapidMsgRosPub::~GenericRapidMsgRosPub() {} -void GenericRapidMsgRosPub::ConvertAdvertisementInfo( +void GenericRapidMsgRosPub::ConvertData( rapid::ext::astrobee::GenericCommsAdvertisementInfo const* data) { const std::lock_guard lock(m_mutex_); @@ -50,7 +50,7 @@ void GenericRapidMsgRosPub::ConvertAdvertisementInfo( } } -void GenericRapidMsgRosPub::ConvertContent( +void GenericRapidMsgRosPub::ConvertData( rapid::ext::astrobee::GenericCommsContent const* data) { const std::lock_guard lock(m_mutex_); diff --git a/communications/comms_bridge/src/generic_rapid_sub.cpp b/communications/comms_bridge/src/generic_rapid_sub.cpp deleted file mode 100644 index 61f1889c65..0000000000 --- a/communications/comms_bridge/src/generic_rapid_sub.cpp +++ /dev/null @@ -1,46 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include "comms_bridge/generic_rapid_sub.h" - -#include - -namespace ff { - -GenericRapidSub::GenericRapidSub(const std::string& entity_name, const std::string& subscribe_topic, - GenericRapidMsgRosPub* rapid_msg_ros_pub) - : dds_event_loop_(entity_name), subscribe_topic_(subscribe_topic), ros_pub_(rapid_msg_ros_pub) {} - -GenericRapidSub::~GenericRapidSub() { - alive_ = false; // Notify thread to exit - thread_.join(); -} - -void GenericRapidSub::StartThread() { - // start joinable thread - thread_ = std::thread(&GenericRapidSub::ThreadExec, this); -} - -void GenericRapidSub::ThreadExec() { - while (alive_) { - // process events at 10hz - dds_event_loop_.processEvents(kn::milliseconds(100)); - } -} - -} // end namespace ff diff --git a/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp b/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp index 4d1666effd..5df2cd0625 100644 --- a/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp +++ b/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp @@ -27,14 +27,14 @@ GenericROSSubRapidPub::GenericROSSubRapidPub() : GenericROSSubRapidPub::~GenericROSSubRapidPub() {} -void GenericROSSubRapidPub::InitializeDDS(std::string agent_name) { +void GenericROSSubRapidPub::InitializeDDS() { advertisement_info_supplier_.reset( new GenericROSSubRapidPub::AdvertisementInfoSupplier( - rapid::ext::astrobee::GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC + agent_name, + rapid::ext::astrobee::GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC, "", "AstrobeeGenericCommsAdvertisementInfoProfile", "")); content_supplier_.reset(new GenericROSSubRapidPub::ContentSupplier( - rapid::ext::astrobee::GENERIC_COMMS_CONTENT_TOPIC + agent_name, + rapid::ext::astrobee::GENERIC_COMMS_CONTENT_TOPIC, "", "AstrobeeGenericCommsContentProfile", "")); rapid::RapidHelper::initHeader(advertisement_info_supplier_->event().hdr); diff --git a/communications/comms_bridge/src/rapid_advertisement_info.cpp b/communications/comms_bridge/src/rapid_advertisement_info.cpp deleted file mode 100644 index 2d3a2a42f9..0000000000 --- a/communications/comms_bridge/src/rapid_advertisement_info.cpp +++ /dev/null @@ -1,55 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include "comms_bridge/rapid_advertisement_info.h" - -#include - -namespace ff { - -RapidAdvertisementInfo::RapidAdvertisementInfo(const std::string& subscribe_topic, - const std::string& subscriber_partition, - GenericRapidMsgRosPub* rapid_msg_ros_pub) - : GenericRapidSub("AstrobeeGenericCommsAdvertisementInfoProfile", subscribe_topic, rapid_msg_ros_pub), - subscriber_partition_(subscriber_partition) { - // connect to ddsEventLoop - try { - dds_event_loop_.connect( - this, - subscribe_topic, // topic - subscriber_partition, // name - "AstrobeeGenericCommsAdvertisementInfo", // profile - ""); // library - } catch (std::exception& e) { - ROS_ERROR_STREAM("RapidAdvertisementInfo exception: " << e.what()); - throw; - } catch (...) { - ROS_ERROR("RapidAdvertisementInfo exception unknown"); - throw; - } - - // start thread - StartThread(); -} - -void RapidAdvertisementInfo::operator() ( - rapid::ext::astrobee::GenericCommsAdvertisementInfo const* data) { - ros_pub_->ConvertAdvertisementInfo(data); -} - -} // end namespace ff diff --git a/communications/comms_bridge/src/ros_ros_bridge_publisher.cpp b/communications/comms_bridge/src/ros_ros_bridge_publisher.cpp deleted file mode 100644 index 40c018622c..0000000000 --- a/communications/comms_bridge/src/ros_ros_bridge_publisher.cpp +++ /dev/null @@ -1,121 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include -#include -#include - -#include "comms_bridge/ros_ros_bridge_publisher.h" - -// ROS internal subscriber message queue size -#define SUBSCRIBER_QUEUE_SIZE 10 -// ROS internal publisher message queue size -#define PUBLISHER_QUEUE_SIZE 10 - -ROSROSBridgePublisher::ROSROSBridgePublisher(const std::string& meta_topic_prefix, double ad2pub_delay) - : BridgePublisher(ad2pub_delay), m_meta_topic_prefix(meta_topic_prefix) { - setupMetaChannels(); - setupReverseMetaChannels(); -} - -ROSROSBridgePublisher::~ROSROSBridgePublisher() {} - -void ROSROSBridgePublisher::setupMetaChannels() { - ros::NodeHandle nh; - - std::string advertisement_topic = m_meta_topic_prefix + "/advert"; - if (m_verbose > 0) - printf("advert topic = %s\n", advertisement_topic.c_str()); - m_advert_sub = - nh.subscribe(advertisement_topic, SUBSCRIBER_QUEUE_SIZE, &ROSROSBridgePublisher::handleAdMessage, this); - - std::string content_topic = m_meta_topic_prefix + "/content"; - if (m_verbose > 0) - printf("content topic = %s\n", content_topic.c_str()); - m_content_sub = - nh.subscribe(content_topic, SUBSCRIBER_QUEUE_SIZE, &ROSROSBridgePublisher::handleContentMessage, this); -} - -void ROSROSBridgePublisher::setupReverseMetaChannels() { - ros::NodeHandle nh; - - std::string reset_topic = m_meta_topic_prefix + "/reset"; - m_reset_pub = nh.advertise(reset_topic, PUBLISHER_QUEUE_SIZE); -} - -void ROSROSBridgePublisher::requestTopicInfo(std::string const& output_topic) { - ff_msgs::RelayReset reset_msg; - - reset_msg.header.seq = 0; - reset_msg.header.stamp = ros::Time::now(); - - reset_msg.topic = output_topic; - - m_reset_pub.publish(reset_msg); -} - -void ROSROSBridgePublisher::handleContentMessage(const ff_msgs::RelayContent::ConstPtr& msg) { - const std::lock_guard lock(m_mutex); - - const std::string output_topic = msg->output_topic; - - if (m_verbose) - printf("Received content message for topic %s\n", output_topic.c_str()); - - std::map::iterator iter = m_relay_topics.find(output_topic); - if (iter == m_relay_topics.end()) { - // first time seeing this topic - // The advertisement that preceded this was either lost, delayed out of - // order, or we the publisher were restarted after it was sent - if (m_verbose) - printf(" first seeing topic\n"); - - RelayTopicInfo topic_info; - topic_info.out_topic = output_topic; - topic_info.ad_info.md5_sum = msg->type_md5_sum; - iter = m_relay_topics.emplace(output_topic, topic_info).first; - - requestTopicInfo(output_topic); - } - - RelayTopicInfo &topic_info = iter->second; - - ContentInfo content_info; - content_info.type_md5_sum = msg->type_md5_sum; - content_info.data = msg->msg; - - if (!relayMessage(topic_info, content_info)) - printf(" error relaying message\n"); -} - -void ROSROSBridgePublisher::handleAdMessage(const ff_msgs::RelayAdvertisement::ConstPtr& msg) { - const std::lock_guard lock(m_mutex); - - const std::string output_topic = msg->output_topic; - - if (m_verbose) - printf("Received advertisement message for topic %s\n", output_topic.c_str()); - - AdvertisementInfo ad_info; - ad_info.md5_sum = msg->md5_sum; - ad_info.data_type = msg->data_type; - ad_info.definition = msg->definition; - - if (!advertiseTopic(msg->output_topic, ad_info)) - printf(" error advertising topic\n"); -} diff --git a/communications/comms_bridge/src/ros_ros_bridge_subscriber.cpp b/communications/comms_bridge/src/ros_ros_bridge_subscriber.cpp deleted file mode 100644 index ef53a7e5b9..0000000000 --- a/communications/comms_bridge/src/ros_ros_bridge_subscriber.cpp +++ /dev/null @@ -1,124 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -#include -#include - -#include "comms_bridge/ros_ros_bridge_subscriber.h" - -// ROS internal subscriber message queue size -#define SUBSCRIBER_QUEUE_SIZE 10 -// ROS internal publisher message queue size -#define PUBLISHER_QUEUE_SIZE 10 - -ROSROSBridgeSubscriber::ROSROSBridgeSubscriber(const std::string& meta_topic_prefix) - : m_meta_topic_prefix(meta_topic_prefix) { - m_n_advertised = 0; - - setupMetaChannels(); - setupReverseMetaChannels(); -} - -ROSROSBridgeSubscriber::~ROSROSBridgeSubscriber() {} - -void ROSROSBridgeSubscriber::setupMetaChannels() { - ros::NodeHandle nh; - - std::string advertisement_topic = m_meta_topic_prefix + "/advert"; - m_advertiser_pub = nh.advertise(advertisement_topic, PUBLISHER_QUEUE_SIZE); - - std::string content_topic = m_meta_topic_prefix + "/content"; - m_relayer_pub = nh.advertise(content_topic, PUBLISHER_QUEUE_SIZE); -} - -void ROSROSBridgeSubscriber::setupReverseMetaChannels() { - ros::NodeHandle nh; - - std::string reset_topic = m_meta_topic_prefix + "/reset"; - m_reset_sub = nh.subscribe(reset_topic, SUBSCRIBER_QUEUE_SIZE, &ROSROSBridgeSubscriber::handleResetMessage, this); -} - -void ROSROSBridgeSubscriber::handleResetMessage(const ff_msgs::RelayReset::ConstPtr& msg) { - const std::string out_topic = msg->topic; - - const std::lock_guard lock(m_mutex); - - std::map::iterator iter = m_relay_topics.begin(); - while (iter != m_relay_topics.end()) { - if (iter->second.out_topic == out_topic) - break; - iter++; - } - - if (iter == m_relay_topics.end()) { - if (m_verbose) - printf("Received reset on unknown topic %s\n", out_topic.c_str()); - return; - } - - if (m_verbose) - printf("Received reset for topic %s\n", out_topic.c_str()); - - requested_resets[out_topic] = true; -} - -void ROSROSBridgeSubscriber::subscribeTopic(std::string const& in_topic, const RelayTopicInfo& info) { - // this is just the base subscriber letting us know it's adding a topic - // nothing more we need to do -} - -void ROSROSBridgeSubscriber::advertiseTopic(const RelayTopicInfo& info) { - const AdvertisementInfo &ad_info = info.ad_info; - - ff_msgs::RelayAdvertisement ad; - - ad.header.seq = ++m_n_advertised; - ad.header.stamp = ros::Time::now(); - - ad.output_topic = info.out_topic; - - ad.latching = ad_info.latching; - - ad.data_type = ad_info.data_type; - ad.md5_sum = ad_info.md5_sum; - ad.definition = ad_info.definition; - - m_advertiser_pub.publish(ad); -} - -void ROSROSBridgeSubscriber::relayMessage(const RelayTopicInfo& topic_info, ContentInfo const& content_info) { - std::map::iterator iter = requested_resets.find(topic_info.out_topic); - if (iter != requested_resets.end()) { - advertiseTopic(topic_info); - requested_resets.erase(iter); - } - - ff_msgs::RelayContent content; - - content.header.seq = topic_info.relay_seqnum; - content.header.stamp = ros::Time::now(); - - content.output_topic = topic_info.out_topic; - - content.type_md5_sum = content_info.type_md5_sum; - - content.msg.resize(content_info.data_size); - std::memcpy(static_cast(content.msg.data()), static_cast(content_info.data), content.msg.size()); - - m_relayer_pub.publish(content); -} diff --git a/communications/dds_msgs/idl/AstrobeeConstants.idl b/communications/dds_msgs/idl/AstrobeeConstants.idl index 8e8cd972f1..9d022e663e 100644 --- a/communications/dds_msgs/idl/AstrobeeConstants.idl +++ b/communications/dds_msgs/idl/AstrobeeConstants.idl @@ -36,7 +36,6 @@ module rapid { const String64 FAULT_STATE_TOPIC = "astrobee_fault_state"; const String64 GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC = "astrobee_generic_comms_advertisement_info"; const String64 GENERIC_COMMS_CONTENT_TOPIC = "astrobee_generic_comms_content"; - const String64 GENERIC_COMMS_RESET_TOPIC = "astrobee_generic_comms_reset"; const String64 GNC_CONTROL_STATE_TOPIC = "astrobee_gnc_control_state"; const String64 GNC_FAM_CMD_STATE_TOPIC = "astrobee_gnc_fam_cmd_state"; const String64 GUEST_SCIENCE_CONFIG_TOPIC = "astrobee_guest_science_config"; diff --git a/communications/dds_msgs/idl/GenericCommsReset.idl b/communications/dds_msgs/idl/GenericCommsReset.idl deleted file mode 100644 index b295a5afff..0000000000 --- a/communications/dds_msgs/idl/GenericCommsReset.idl +++ /dev/null @@ -1,33 +0,0 @@ -/* - * Copyright 2023 (c) 2023 Intelligent Robotics Group, NASA ARC - */ - -#include "Message.idl" - -module rapid { - module ext { - module astrobee { - - //@copy-c-declaration class GenericCommsResetTypeSupport; - //@copy-c-declaration class GenericCommsResetDataWriter; - //@copy-c-declaration class GenericCommsResetDataReader; - //@copy-c-declaration struct GenericConmmsResetSeq; - - //@copy-declaration /** - //@copy-declaration * Reset message in case publisher didn't receive initial advertisement info message for this topic - //@copy-declaration */ - valuetype GenericCommsReset : Message { - //@copy-c-declaration #if RTI_DDS_VERSION_MAJOR < 4 || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR < 5) || (RTI_DDS_VERSION_MAJOR == 4 && RTI_DDS_VERSION_MINOR == 5 && RTI_DDS_VERSION_RELEASE != 'f' ) - //@copy-c-declaration typedef GenericCommsResetTypeSupport TypeSupport; - //@copy-c-declaration typedef GenericCommsResetDataWriter DataWriter; - //@copy-c-declaration typedef GenericCommsResetDataReader DataReader; - //@copy-c-declaration typedef GenericCommsResetSeq Seq; - //@copy-c-declaration #endif - //@copy-c-declaration typedef GenericCommsReset Type; - - //@copy-declaration // Topic for which metadata retransmission is requested - public String128 topic; - }; - }; - }; -}; From 40cc3912b2cdd43d36bba9572f20090c0a919322 Mon Sep 17 00:00:00 2001 From: Katie Hamilton Date: Mon, 20 Nov 2023 10:50:09 -0800 Subject: [PATCH 12/32] Added debug. --- .../comms_bridge/include/comms_bridge/generic_rapid_sub.h | 2 ++ communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp | 4 ++-- communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp | 4 ++++ 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h b/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h index fcd09522de..31ddae00bf 100644 --- a/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h +++ b/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h @@ -73,6 +73,8 @@ class GenericRapidSub { } void operator() (T const* data) { +#include "comms_bridge/util.h" + ROS_INFO("Received data for topic %s\n", subscribe_topic_.c_str()); ros_pub_->ConvertData(data); } diff --git a/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp index f981dca3fc..e709a0cd02 100644 --- a/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp +++ b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp @@ -35,7 +35,7 @@ void GenericRapidMsgRosPub::ConvertData( const std::string output_topic = data->outputTopic; - ROS_DEBUG("Comms Bridge Nodelet: Received advertisement message for topic %s\n", + ROS_INFO("Comms Bridge Nodelet: Received advertisement message for topic %s\n", output_topic.c_str()); AdvertisementInfo ad_info; @@ -56,7 +56,7 @@ void GenericRapidMsgRosPub::ConvertData( const std::string output_topic = data->outputTopic; - ROS_DEBUG("Comms Bridge Nodelet: Received content message for topic %s\n", + ROS_INFO("Comms Bridge Nodelet: Received content message for topic %s\n", output_topic.c_str()); std::map::iterator iter = m_relay_topics_.find(output_topic); diff --git a/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp b/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp index 5df2cd0625..0e402598c9 100644 --- a/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp +++ b/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp @@ -57,6 +57,8 @@ void GenericROSSubRapidPub::advertiseTopic(const RelayTopicInfo& relay_info) { unsigned int size; std::string out_topic = relay_info.out_topic; + ROS_INFO("Received ros advertise topic for topic %s\n", out_topic.c_str()); + msg.hdr.timeStamp = comms_util::RosTime2RapidTime(ros::Time::now()); msg.hdr.serial = ++advertisement_info_seq_; @@ -93,6 +95,8 @@ void GenericROSSubRapidPub::relayMessage(const RelayTopicInfo& topic_info, Conte unsigned int size; std::string out_topic = topic_info.out_topic; + ROS_INFO("Received ros content message for topic %s\n", out_topic); + msg.hdr.timeStamp = comms_util::RosTime2RapidTime(ros::Time::now()); msg.hdr.serial = topic_info.relay_seqnum; From e9d9d6a9d94e187e5b5423520fb4a689ec3ddf2c Mon Sep 17 00:00:00 2001 From: Katie Hamilton Date: Mon, 20 Nov 2023 10:51:41 -0800 Subject: [PATCH 13/32] Remove seperate publisher and subscriber nodelet. --- astrobee/launch/robot/MLP.launch | 14 ++------------ .../comms_bridge/launch/comms_bridge.launch | 9 ++------- 2 files changed, 4 insertions(+), 19 deletions(-) diff --git a/astrobee/launch/robot/MLP.launch b/astrobee/launch/robot/MLP.launch index 0eda37a6fc..809f4f77d3 100644 --- a/astrobee/launch/robot/MLP.launch +++ b/astrobee/launch/robot/MLP.launch @@ -297,18 +297,8 @@ - - - - - - - - - - - - + + diff --git a/communications/comms_bridge/launch/comms_bridge.launch b/communications/comms_bridge/launch/comms_bridge.launch index 8023e8ae6a..07ec93abbf 100644 --- a/communications/comms_bridge/launch/comms_bridge.launch +++ b/communications/comms_bridge/launch/comms_bridge.launch @@ -19,14 +19,9 @@ - + - - - - - - + From fcd10548fbdf911e95089fba2e40e2ce31399ab5 Mon Sep 17 00:00:00 2001 From: Katie Hamilton Date: Mon, 20 Nov 2023 17:24:29 -0800 Subject: [PATCH 14/32] Fixed comms bridge seg fault. --- astrobee/config/management/fault_table.config | 3 -- astrobee/launch/robot/MLP.launch | 4 +- .../comms_bridge/src/comms_bridge_nodelet.cpp | 43 +++++++++++++------ .../src/generic_rapid_msg_ros_pub.cpp | 2 +- .../src/generic_ros_sub_rapid_pub.cpp | 17 ++++---- .../idl/GenericCommsAdvertisementInfo.idl | 5 ++- .../dds_msgs/idl/GenericCommsContent.idl | 2 +- 7 files changed, 45 insertions(+), 31 deletions(-) diff --git a/astrobee/config/management/fault_table.config b/astrobee/config/management/fault_table.config index d1f0f7cf1d..64ee4cd3ad 100644 --- a/astrobee/config/management/fault_table.config +++ b/astrobee/config/management/fault_table.config @@ -110,9 +110,6 @@ subsystems={ }}, }}, {name="communication", nodes={ - {name="astrobee_astrobee_bridge", faults={ - {id=51, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Atrobee to Astrobee Bridge", heartbeat={timeout_sec=1.1, misses=1.0}}, - }}, {name="dds_ros_bridge", faults={ {id=33, warning=false, blocking=true, response=command("fault"), key="HEARTBEAT_MISSING", description="No Heartbeat form DDS ROS Bridge", heartbeat={timeout_sec=1.1, misses=5.0}}, {id=78, warning=false, blocking=true, response=command("unloadNodelet", "dds_ros_bridge", ""), key="INITIALIZATION_FAILED", description="DDS ROS Bridge Initialization Failed"}, diff --git a/astrobee/launch/robot/MLP.launch b/astrobee/launch/robot/MLP.launch index 809f4f77d3..9d31a11d73 100644 --- a/astrobee/launch/robot/MLP.launch +++ b/astrobee/launch/robot/MLP.launch @@ -286,7 +286,7 @@ - + diff --git a/communications/comms_bridge/src/comms_bridge_nodelet.cpp b/communications/comms_bridge/src/comms_bridge_nodelet.cpp index dc3d462088..9009a4b899 100644 --- a/communications/comms_bridge/src/comms_bridge_nodelet.cpp +++ b/communications/comms_bridge/src/comms_bridge_nodelet.cpp @@ -136,6 +136,8 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { robot_params->name = agent_name_; robot_params->namingContextName = robot_params->name; + ROS_ERROR("Agent name %s and participant name %s\n", agent_name_.c_str(), participant_name_.c_str()); + // Set values for default punlisher and subscriber dds_params->publishers[0].name = agent_name_; dds_params->publishers[0].partition = agent_name_; @@ -161,7 +163,7 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { } // Register the connections into the parameters so they can be used later - for (int i = 0; i <= rapid_connections_.size(); i++) { + for (int i = 0; i < rapid_connections_.size(); i++) { // This shouldn't be needed but check just in case if (local_subscriber != rapid_connections_[i]) { kn::DdsNodeParameters subscriber; @@ -196,7 +198,6 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { for (size_t i = 0; i < rapid_connections_.size(); i++) { // Lower case the external agent name to use it like a namespace connection = rapid_connections_[i]; - connection[0] = tolower(connection[0]); advertisement_info_sub = std::make_shared>( "AstrobeeGenericCommsAdvertisementInfoProfile", rapid::ext::astrobee::GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC, @@ -210,6 +211,13 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { ros_pub_.get()); content_rapid_subs_.push_back(content_sub); } + + std::string ns = std::string("/") + agent_name_ + "/"; + ns[1] = std::tolower(ns[1]); // namespaces don't start with upper case + + for (size_t i = 0; i < topics_sub_.size(); i++) { + ros_sub_.addTopic(topics_sub_[i], (ns + topics_sub_[i])); + } } bool ReadParams() { @@ -228,9 +236,6 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { ros_sub_.setVerbosity(verbose); ros_pub_->setVerbosity(verbose); - std::string ns = std::string("/") + agent_name_ + "/"; - ns[1] = std::tolower(ns[1]); // namespaces don't start with upper case - // Load shared topic groups config_reader::ConfigReader::Table links, link; if (!config_params_.GetTable("links", &links)) { @@ -246,21 +251,22 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { std::string config_agent; if (link.GetStr("from", &config_agent) && config_agent == agent_name_) { AddRapidConnections(link, "to"); - AddTableToSubs(link, "relay_forward", ns); - AddTableToSubs(link, "relay_both", ns); + AddTableToSubs(link, "relay_forward"); + AddTableToSubs(link, "relay_both"); } else if (link.GetStr("to", &config_agent) && config_agent == agent_name_) { AddRapidConnections(link, "from"); - AddTableToSubs(link, "relay_backward", ns); - AddTableToSubs(link, "relay_both", ns); + AddTableToSubs(link, "relay_backward"); + AddTableToSubs(link, "relay_both"); } } + return true; } void AddRapidConnections(config_reader::ConfigReader::Table &link_table, std::string direction) { std::string connection; if (!link_table.GetStr(direction.c_str(), &connection)) { - NODELET_ERROR("Comms Bridge Nodelet: %s not specified for one link", direction); + NODELET_ERROR("Comms Bridge Nodelet: %s not specified for one link", direction.c_str()); return; } @@ -278,8 +284,7 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { } void AddTableToSubs(config_reader::ConfigReader::Table &link_table, - std::string table_name, - std::string ns) { + std::string table_name) { config_reader::ConfigReader::Table relay_table, relay_item; std::string topic_name; if (link_table.GetTable(table_name.c_str(), &relay_table)) { @@ -289,7 +294,17 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { NODELET_ERROR("Comms Bridge Nodelet: Agent topic name not specified!"); continue; } - ros_sub_.addTopic(topic_name, (ns + topic_name)); + + bool found = false; + for (size_t i = 0; i < topics_sub_.size() && !found; i++) { + if (topic_name == topics_sub_[i]) { + found = true; + } + } + + if (!found) { + topics_sub_.push_back(topic_name); + } } } } @@ -302,7 +317,7 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { std::shared_ptr dds_entities_factory_; std::shared_ptr ros_pub_; std::string agent_name_, participant_name_; - std::vector rapid_connections_; + std::vector rapid_connections_, topics_sub_; }; PLUGINLIB_EXPORT_CLASS(comms_bridge::CommsBridgeNodelet, nodelet::Nodelet) diff --git a/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp index e709a0cd02..4a130efe1f 100644 --- a/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp +++ b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp @@ -62,7 +62,7 @@ void GenericRapidMsgRosPub::ConvertData( std::map::iterator iter = m_relay_topics_.find(output_topic); if (iter == m_relay_topics_.end()) { ROS_ERROR("Comms Bridge Nodelet: Received content for topic %s but never received advertisement info.\n", - output_topic); + output_topic.c_str()); } else { RelayTopicInfo &topic_info = iter->second; diff --git a/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp b/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp index 0e402598c9..42549fc640 100644 --- a/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp +++ b/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp @@ -57,7 +57,7 @@ void GenericROSSubRapidPub::advertiseTopic(const RelayTopicInfo& relay_info) { unsigned int size; std::string out_topic = relay_info.out_topic; - ROS_INFO("Received ros advertise topic for topic %s\n", out_topic.c_str()); + ROS_ERROR("Received ros advertise topic for topic %s\n", out_topic.c_str()); msg.hdr.timeStamp = comms_util::RosTime2RapidTime(ros::Time::now()); msg.hdr.serial = ++advertisement_info_seq_; @@ -74,13 +74,14 @@ void GenericROSSubRapidPub::advertiseTopic(const RelayTopicInfo& relay_info) { std::strncpy(msg.dataType, info.data_type.data(), size); msg.dataType[size] = '\0'; - // Currently the md5 sum can only be 32 characters long - SizeCheck(size, info.md5_sum.size(), 32, "MD5 sum", out_topic); + // Currently the md5 sum can only be 64 characters long + SizeCheck(size, info.md5_sum.size(), 64, "MD5 sum", out_topic); std::strncpy(msg.md5Sum, info.md5_sum.data(), size); msg.md5Sum[size] = '\0'; - // Current the ROS message definition can only be 2048 characters long - SizeCheck(size, info.definition.size(), 2048, "Msg definition", out_topic); + ROS_ERROR("Defination is %i long\n", info.definition.size()); + // Current the ROS message definition can only be 16384 characters long + SizeCheck(size, info.definition.size(), 16384, "Msg definition", out_topic); std::strncpy(msg.msgDefinition, info.definition.data(), size); msg.msgDefinition[size] = '\0'; @@ -95,7 +96,7 @@ void GenericROSSubRapidPub::relayMessage(const RelayTopicInfo& topic_info, Conte unsigned int size; std::string out_topic = topic_info.out_topic; - ROS_INFO("Received ros content message for topic %s\n", out_topic); + ROS_ERROR("Received ros content message for topic %s\n", out_topic.c_str()); msg.hdr.timeStamp = comms_util::RosTime2RapidTime(ros::Time::now()); msg.hdr.serial = topic_info.relay_seqnum; @@ -105,8 +106,8 @@ void GenericROSSubRapidPub::relayMessage(const RelayTopicInfo& topic_info, Conte std::strncpy(msg.outputTopic, out_topic.data(), size); msg.outputTopic[size] = '\0'; - // Currently the md5 sum can only be 32 characters long - SizeCheck(size, content_info.type_md5_sum.size(), 32, "MD5 sum", out_topic); + // Currently the md5 sum can only be 64 characters long + SizeCheck(size, content_info.type_md5_sum.size(), 64, "MD5 sum", out_topic); std::strncpy(msg.md5Sum, content_info.type_md5_sum.data(), size); msg.md5Sum[size] = '\0'; diff --git a/communications/dds_msgs/idl/GenericCommsAdvertisementInfo.idl b/communications/dds_msgs/idl/GenericCommsAdvertisementInfo.idl index 0deeef6fcd..2950f0a493 100644 --- a/communications/dds_msgs/idl/GenericCommsAdvertisementInfo.idl +++ b/communications/dds_msgs/idl/GenericCommsAdvertisementInfo.idl @@ -7,6 +7,7 @@ module rapid { module ext { module astrobee { + typedef string<16384> String16K; //@copy-c-declaration class GenericCommsAdvertisementInfoTypeSupport; //@copy-c-declaration class GenericCommsAdvertisementInfoDataWriter; @@ -35,10 +36,10 @@ module rapid { public String128 dataType; //@copy-declaration /** ROS message md5sum of type */ - public String32 md5Sum; + public String64 md5Sum; //@copy-declaration /** ROS message definition */ - public String2K msgDefinition; + public String16K msgDefinition; }; }; }; diff --git a/communications/dds_msgs/idl/GenericCommsContent.idl b/communications/dds_msgs/idl/GenericCommsContent.idl index 5a44680192..5ab9cd5a20 100644 --- a/communications/dds_msgs/idl/GenericCommsContent.idl +++ b/communications/dds_msgs/idl/GenericCommsContent.idl @@ -29,7 +29,7 @@ module rapid { public String128 outputTopic; //@copy-declaration /** The md5 sum of msg type repeated from the original advertisement */ - public String32 md5Sum; + public String64 md5Sum; //@copy-declaration /** Serialized content of the message */ public OctetSequence128K data; From fa6c92d255e65796a50c004378d04b5f7a0a5f1d Mon Sep 17 00:00:00 2001 From: Katie Hamilton Date: Tue, 21 Nov 2023 09:04:36 -0800 Subject: [PATCH 15/32] More debug output. --- .../comms_bridge/include/comms_bridge/generic_rapid_sub.h | 3 +-- communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp | 4 ++-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h b/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h index 31ddae00bf..1cfd956015 100644 --- a/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h +++ b/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h @@ -73,8 +73,7 @@ class GenericRapidSub { } void operator() (T const* data) { -#include "comms_bridge/util.h" - ROS_INFO("Received data for topic %s\n", subscribe_topic_.c_str()); + ROS_ERROR("Received data for topic %s\n", subscribe_topic_.c_str()); ros_pub_->ConvertData(data); } diff --git a/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp index 4a130efe1f..ae5088933a 100644 --- a/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp +++ b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp @@ -35,7 +35,7 @@ void GenericRapidMsgRosPub::ConvertData( const std::string output_topic = data->outputTopic; - ROS_INFO("Comms Bridge Nodelet: Received advertisement message for topic %s\n", + ROS_ERROR("Comms Bridge Nodelet: Received advertisement message for topic %s\n", output_topic.c_str()); AdvertisementInfo ad_info; @@ -56,7 +56,7 @@ void GenericRapidMsgRosPub::ConvertData( const std::string output_topic = data->outputTopic; - ROS_INFO("Comms Bridge Nodelet: Received content message for topic %s\n", + ROS_ERROR("Comms Bridge Nodelet: Received content message for topic %s\n", output_topic.c_str()); std::map::iterator iter = m_relay_topics_.find(output_topic); From ec3b63355ecda8e2814f9279bc4cc8706be2542e Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Tue, 21 Nov 2023 17:38:02 -0800 Subject: [PATCH 16/32] after testing adding info --- .../config/communications/comms_bridge.config | 24 ++++--------------- .../comms_bridge/src/comms_bridge_nodelet.cpp | 7 +++++- .../src/generic_rapid_msg_ros_pub.cpp | 6 +++++ 3 files changed, 17 insertions(+), 20 deletions(-) diff --git a/astrobee/config/communications/comms_bridge.config b/astrobee/config/communications/comms_bridge.config index 331bfb079a..3c0f86a4b3 100644 --- a/astrobee/config/communications/comms_bridge.config +++ b/astrobee/config/communications/comms_bridge.config @@ -38,35 +38,21 @@ links = { -- A single link entry has required fields "from" and "to" that specify the robot roles involved -- in the link. from = "Bumble", -- manager - to = "Queen", -- actor + to = "Wannabee", -- actor -- The link entry has three optional fields: relay_forward (messages to be relayed only in the -- `from` to `to` direction), relay_backward (to be relayed only in the `to` to `from` direction), -- and relay_both (to be relayed in both directions). Providing all three fields gives the user -- full directional control while minimizing repetition and copy/paste errors. relay_forward = { - {name = "beh/inspection/goal"}, - {name = "beh/dock/goal"}, - {name = "gnc/control/goal"}, + {name = "mgt/executive/agent_state"}, }, relay_backward = { - {name = "beh/inspection/result"}, - {name = "beh/dock/result"}, - {name = "gnc/control/result"}, + {name = "dummy1"}, }, relay_both = { - {name = "beh/inspection/cancel"}, - {name = "beh/inspection/feedback"}, - {name = "beh/inspection/state"}, - {name = "beh/inspection/status"}, - {name = "beh/dock/cancel"}, - {name = "beh/dock/feedback"}, - {name = "beh/dock/state"}, - {name = "beh/dock/status"}, - {name = "gnc/control/cancel"}, - {name = "gnc/control/feedback"}, - {name = "gnc/control/status"}, - } + {name = "dummy2"}, + }, } } diff --git a/communications/comms_bridge/src/comms_bridge_nodelet.cpp b/communications/comms_bridge/src/comms_bridge_nodelet.cpp index 9009a4b899..f548648434 100644 --- a/communications/comms_bridge/src/comms_bridge_nodelet.cpp +++ b/communications/comms_bridge/src/comms_bridge_nodelet.cpp @@ -100,6 +100,7 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { agent_name_[0] = toupper(agent_name_[0]); } } + ROS_INFO_STREAM("Comms Bridge Nodelet: agent name " << agent_name_); int fake_argc = 1; @@ -136,7 +137,7 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { robot_params->name = agent_name_; robot_params->namingContextName = robot_params->name; - ROS_ERROR("Agent name %s and participant name %s\n", agent_name_.c_str(), participant_name_.c_str()); + ROS_INFO("Agent name %s and participant name %s\n", agent_name_.c_str(), participant_name_.c_str()); // Set values for default punlisher and subscriber dds_params->publishers[0].name = agent_name_; @@ -216,6 +217,7 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { ns[1] = std::tolower(ns[1]); // namespaces don't start with upper case for (size_t i = 0; i < topics_sub_.size(); i++) { + ROS_INFO_STREAM("Initialize DDS topic sub: " << topics_sub_[i]); ros_sub_.addTopic(topics_sub_[i], (ns + topics_sub_[i])); } } @@ -243,12 +245,15 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { return false; } + ROS_INFO_STREAM("Read Params numebr of links: " << links.GetSize()); for (int i = 1; i <= links.GetSize(); i++) { if (!links.GetTable(i, &link)) { NODELET_ERROR("Comms Bridge Nodelet: Could read link table row %i", i); continue; } std::string config_agent; + ROS_INFO_STREAM("Link " << i << " from " << link.GetStr("from", &config_agent)); + ROS_INFO_STREAM("Link " << i << " to " << link.GetStr("to", &config_agent)); if (link.GetStr("from", &config_agent) && config_agent == agent_name_) { AddRapidConnections(link, "to"); AddTableToSubs(link, "relay_forward"); diff --git a/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp index 4a130efe1f..44be2c5a3c 100644 --- a/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp +++ b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp @@ -29,6 +29,7 @@ GenericRapidMsgRosPub::GenericRapidMsgRosPub(double ad2pub_delay) : GenericRapidMsgRosPub::~GenericRapidMsgRosPub() {} +// Handle Ad Message void GenericRapidMsgRosPub::ConvertData( rapid::ext::astrobee::GenericCommsAdvertisementInfo const* data) { const std::lock_guard lock(m_mutex_); @@ -43,6 +44,10 @@ void GenericRapidMsgRosPub::ConvertData( ad_info.data_type = data->dataType; ad_info.md5_sum = data->md5Sum; ad_info.definition = data->msgDefinition; + ROS_INFO_STREAM("ad_info latching: " << data->latching); + ROS_INFO_STREAM("ad_info dataType: " << data->dataType); + ROS_INFO_STREAM("ad_info md5Sum: " << data->md5Sum); + ROS_INFO_STREAM("ad_info msgDefinition: " << data->msgDefinition); if (!advertiseTopic(output_topic, ad_info)) { ROS_ERROR("Comms Bridge Nodelet: Error advertising topic: %s\n", @@ -50,6 +55,7 @@ void GenericRapidMsgRosPub::ConvertData( } } +// Handle content message void GenericRapidMsgRosPub::ConvertData( rapid::ext::astrobee::GenericCommsContent const* data) { const std::lock_guard lock(m_mutex_); From b9e93fac8040e13a0e361f819aa6342a28323b90 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Tue, 21 Nov 2023 17:45:48 -0800 Subject: [PATCH 17/32] fixing docs --- .../config/communications/comms_bridge.config | 11 ++-------- communications/comms_bridge/README.md | 21 ++++--------------- 2 files changed, 6 insertions(+), 26 deletions(-) diff --git a/astrobee/config/communications/comms_bridge.config b/astrobee/config/communications/comms_bridge.config index 3c0f86a4b3..0261712519 100644 --- a/astrobee/config/communications/comms_bridge.config +++ b/astrobee/config/communications/comms_bridge.config @@ -15,18 +15,11 @@ -- License for the specific language governing permissions and limitations -- under the License. --- param for each bridge component use the following params -- --- ABREV = First Letters of Component Ex: ros_string_rapid_text_msg = RSRTM -- --- use_ABREV bool: turn this component on? -- --- sub_topic_ABREV string: [ROS: subscribe topic] or [RAPID: subscribe topic suffix] -- --- pub_topic_ABREV string: [ROS: publish topic] or [RAPID: publish topic suffix] -- --- queue_size_ABREV int: queue size -- - require "context" -- Enable DDS communications on start -- -- If false, a trigger call will be needed to start communications -- -started = false; +started = false -- Rapid subscribers -- rapid_sub_names = {"Bumble", "Honey", "Queen", "Bsharp", "Wannabee"} @@ -57,4 +50,4 @@ links = { } verbose = 2 -ad2pub_delay = 3.0 \ No newline at end of file +ad2pub_delay = 3.0 diff --git a/communications/comms_bridge/README.md b/communications/comms_bridge/README.md index 43ec478cef..d13d6f3803 100644 --- a/communications/comms_bridge/README.md +++ b/communications/comms_bridge/README.md @@ -1,26 +1,13 @@ -\page coms_bridge Modular Polymorphic ROS Relay +\page comms_bridge DDS Generic Comms Bridge -See that dir for architecture info. - -This is unidirectional, supporting an arbitrary number of in-out topics. -For bi-directional, an additional paired set of flipped subscribers/publishers can be run. - -This is a demonstration of unidirectional relaying of ROS messages from one topic to another, without any specification of the message types, nor even the requirement that the types be available to the nodes at compile-time. - -This is a demonstration insofar as the metadata backchannel is itself just ROS topics on the same ROS master, but this can be replaced with other conduits. +This is bidirectional bridge to communicate between different astrobees or ros clients using DDS to relay the topics. Supporting an arbitrary number of in-out topics, relaying of ROS messages from one topic to another, without any specification of the message types, nor even the requirement that the types be available to the nodes at compile-time. ## Basic architecture -- The subscriber node (A) subscribes to a specified list of topics +- The subscriber class (A) subscribes to a specified list of topics - Upon receipt of the first message on a given forwarded topic, A sends an advertisement message to a metadata "advertisement" topic -- Receiving the metadata advertisement, the republisher node (B) itself advertises the output (republished) topic +- Receiving the metadata advertisement, the republisher class (B) itself advertises the output (republished) topic - The first and all subsequent messages A receives are serialized and published to a metadata "content" topic - A delay is implemented in B between its advertising and subsequent republishing of any messages to practically avoid the potential race condition of a subscriber on that end missing the first message as it connects to B only upon seeing the advertisement - Otherwise, B deserializes all messages received on the "content" metadata topic, looks up the topic in previously received "advertisement" metadata, and republishes the message on the output topic - -## Reverse channel - -To account for situations in which the republisher node may be restarted after the subscriber node has already been running, or to handle implementations in which advertisement metadata could be lost (eg if the metadata channels were implemented on a lossy medium), the republisher can request retransmission of "advertisement" metadata for any topic. It does this by publishing a message to a "reset" metadata channel, specifying the topic name, to which the the subscriber node listens and retransmits the advertisement. - - From 2b2210bd269374c0948e1181612871da7c70b81c Mon Sep 17 00:00:00 2001 From: Kathryn Hamilton Date: Mon, 18 Dec 2023 17:35:34 -0800 Subject: [PATCH 18/32] Progress update where connection both ways works. --- .../include/comms_bridge/generic_rapid_pub.h | 80 +++++++++ .../comms_bridge/generic_ros_sub_rapid_pub.h | 38 ++-- .../comms_bridge/src/comms_bridge_nodelet.cpp | 20 ++- .../comms_bridge/src/generic_rapid_pub.cpp | 168 ++++++++++++++++++ .../src/generic_ros_sub_rapid_pub.cpp | 131 +++++--------- .../idl/GenericCommsAdvertisementInfo.idl | 4 +- 6 files changed, 320 insertions(+), 121 deletions(-) create mode 100644 communications/comms_bridge/include/comms_bridge/generic_rapid_pub.h create mode 100644 communications/comms_bridge/src/generic_rapid_pub.cpp diff --git a/communications/comms_bridge/include/comms_bridge/generic_rapid_pub.h b/communications/comms_bridge/include/comms_bridge/generic_rapid_pub.h new file mode 100644 index 0000000000..9e135cda73 --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/generic_rapid_pub.h @@ -0,0 +1,80 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#ifndef COMMS_BRIDGE_GENERIC_RAPID_PUB_H_ +#define COMMS_BRIDGE_GENERIC_RAPID_PUB_H_ + +#include +#include + +#include + +#include "knDds/DdsTypedSupplier.h" + +#include "rapidUtil/RapidHelper.h" + +#include "dds_msgs/AstrobeeConstantsSupport.h" +#include "dds_msgs/GenericCommsAdvertisementInfoSupport.h" +#include "dds_msgs/GenericCommsContentSupport.h" + +namespace ff { + +class GenericRapidPub { + public: + explicit GenericRapidPub(std::string const& robot_name); + ~GenericRapidPub(); + + template + void CopyString(const int max_size, + std::string src, + T &dest, + std::string const& data_name, + std::string const& topic); + + void ProcessAdvertisementInfo(std::string const& output_topic, + bool latching, + std::string const& data_type, + std::string const& md5_sum, + std::string definition); + + void ProcessContent(std::string const& output_topic, + std::string const& md5_sum, + uint8_t const* data, + const size_t data_size, + const int seq_num); + + private: + using AdvertisementInfoSupplier = + kn::DdsTypedSupplier; + using AdvertisementInfoSupplierPtr = + std::unique_ptr; + AdvertisementInfoSupplierPtr advertisement_info_supplier_; + + using ContentSupplier = + kn::DdsTypedSupplier; + using ContentSupplierPtr = std::unique_ptr; + ContentSupplierPtr content_supplier_; + + unsigned int advertisement_info_seq_; +}; + +typedef std::shared_ptr GenericRapidPubPtr; + +} // end namespace ff + +#endif // COMMS_BRIDGE_GENERIC_RAPID_PUB_H_ diff --git a/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h b/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h index 42d8ee0205..c0cc6db79e 100644 --- a/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h +++ b/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h @@ -23,21 +23,16 @@ */ #include -#include +#include #include #include #include +#include #include - -#include "knDds/DdsTypedSupplier.h" - -#include "rapidUtil/RapidHelper.h" - -#include "dds_msgs/AstrobeeConstantsSupport.h" -#include "dds_msgs/GenericCommsAdvertisementInfoSupport.h" -#include "dds_msgs/GenericCommsContentSupport.h" +#include +#include namespace ff { @@ -46,13 +41,10 @@ class GenericROSSubRapidPub : public BridgeSubscriber { GenericROSSubRapidPub(); ~GenericROSSubRapidPub(); - void InitializeDDS(); + void AddTopics(std::map>> const& link_entries); - void SizeCheck(unsigned int &size, - const int size_in, - const int max_size, - std::string const& data_name, - std::string const& topic); + void InitializeDDS(std::vector const& connections); // Called with the mutex held virtual void subscribeTopic(std::string const& in_topic, const RelayTopicInfo& info); @@ -66,19 +58,9 @@ class GenericROSSubRapidPub : public BridgeSubscriber { private: bool dds_initialized_; - using AdvertisementInfoSupplier = - kn::DdsTypedSupplier; - using AdvertisementInfoSupplierPtr = std::unique_ptr; - - AdvertisementInfoSupplierPtr advertisement_info_supplier_; - - using ContentSupplier = - kn::DdsTypedSupplier; - using ContentSupplierPtr = std::unique_ptr; - - ContentSupplierPtr content_supplier_; - - unsigned int advertisement_info_seq_; + std::map>> topic_mapping_; + std::map robot_connections_; + std::string robot_name_; }; } // end namespace ff diff --git a/communications/comms_bridge/src/comms_bridge_nodelet.cpp b/communications/comms_bridge/src/comms_bridge_nodelet.cpp index f548648434..dd3a40b6ea 100644 --- a/communications/comms_bridge/src/comms_bridge_nodelet.cpp +++ b/communications/comms_bridge/src/comms_bridge_nodelet.cpp @@ -192,22 +192,32 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { } void InitializeDDS() { - std::string connection; + std::string connection, dds_topic_name; ff::AdvertisementInfoRapidSubPtr advertisement_info_sub; ff::ContentRapidSubPtr content_sub; - ros_sub_.InitializeDDS(); + ros_sub_.InitializeDDS(rapid_connections_); for (size_t i = 0; i < rapid_connections_.size(); i++) { // Lower case the external agent name to use it like a namespace connection = rapid_connections_[i]; + dds_topic_name = agent_name_ + "-" + + rapid::ext::astrobee::GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC; + ROS_ERROR("Comms Bridge: DDS Sub DDS advertisement info topic name: %s\n", + dds_topic_name.c_str()); advertisement_info_sub = std::make_shared>( - "AstrobeeGenericCommsAdvertisementInfoProfile", rapid::ext::astrobee::GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC, - connection, ros_pub_.get()); + "AstrobeeGenericCommsAdvertisementInfoProfile", + dds_topic_name, + connection, + ros_pub_.get()); advertisement_info_rapid_subs_.push_back(advertisement_info_sub); + dds_topic_name = agent_name_ + "-" + + rapid::ext::astrobee::GENERIC_COMMS_CONTENT_TOPIC; + ROS_ERROR("Comms Bridge: DDS Sub DDS content topic name: %s\n", + dds_topic_name.c_str()); content_sub = std::make_shared>( "AstrobeeGenericCommsContentProfile", - rapid::ext::astrobee::GENERIC_COMMS_CONTENT_TOPIC, + dds_topic_name, connection, ros_pub_.get()); content_rapid_subs_.push_back(content_sub); diff --git a/communications/comms_bridge/src/generic_rapid_pub.cpp b/communications/comms_bridge/src/generic_rapid_pub.cpp new file mode 100644 index 0000000000..58895b84af --- /dev/null +++ b/communications/comms_bridge/src/generic_rapid_pub.cpp @@ -0,0 +1,168 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform is licensed under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + */ + +#include "comms_bridge/generic_rapid_pub.h" + +#include + +namespace ff { + +GenericRapidPub::GenericRapidPub(std::string const& robot_name) : + advertisement_info_seq_(0) { + std::string dds_topic_name; + dds_topic_name = robot_name + "-" + + rapid::ext::astrobee::GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC; + ROS_ERROR("Comms Bridge: DDS Pub DDS advertisement info topic name: %s\n", + dds_topic_name.c_str()); + advertisement_info_supplier_.reset( + new GenericRapidPub::AdvertisementInfoSupplier( + dds_topic_name, "", "AstrobeeGenericCommsAdvertisementInfoProfile", "")); + + dds_topic_name = robot_name + "-" + + rapid::ext::astrobee::GENERIC_COMMS_CONTENT_TOPIC; + ROS_ERROR("Comms Bridge: DDS Publisher DDS content topic name: %s\n", + dds_topic_name.c_str()); + content_supplier_.reset(new GenericRapidPub::ContentSupplier( + dds_topic_name, "", "AstrobeeGenericCommsContentProfile", "")); + + rapid::RapidHelper::initHeader(advertisement_info_supplier_->event().hdr); + rapid::RapidHelper::initHeader(content_supplier_->event().hdr); +} + +GenericRapidPub::~GenericRapidPub() {} + +template +void GenericRapidPub::CopyString(const int max_size, + std::string src, + T &dest, + std::string const& data_name, + std::string const& topic) { + unsigned int size = src.size(); + if (size > (max_size - 1)) { + ROS_ERROR("Comms Bridge: %s for topic %s is greater than %i characters. Please change idl.", + data_name.c_str(), topic.c_str(), max_size); + size = (max_size - 1); + } + memset(dest, 0, max_size); + strncpy(dest, src.data(), size); + dest[size] = '\0'; +} + +void GenericRapidPub::ProcessAdvertisementInfo(std::string const& output_topic, + bool latching, + std::string const& data_type, + std::string const& md5_sum, + std::string definition) { + rapid::ext::astrobee::GenericCommsAdvertisementInfo &msg = + advertisement_info_supplier_->event(); + + msg.hdr.timeStamp = comms_util::RosTime2RapidTime(ros::Time::now()); + msg.hdr.serial = ++advertisement_info_seq_; + + // Currrently the output topic can only be 128 characters long + CopyString(128, + output_topic, + msg.outputTopic, + "Out topic", + output_topic); + + msg.latching = latching; + + // Currently the data type can only be 128 characters long + CopyString(128, + data_type, + msg.dataType, + "Data type", + output_topic); + + // Currently the md5 sum can only be 64 characters long + CopyString(64, + md5_sum, + msg.md5Sum, + "Md5 sum", + output_topic); + + // Remove legal statement(s) + std::string::size_type begin = definition.find("# Copyright (c) 20"); + std::string::size_type end; + while (begin != definition.npos) { + end = definition.find("# under the License."); + end += 20; + definition.erase(begin, (end - begin)); + begin = definition.find("# Copyright (c) 20"); + } + + // Currently the ROS message definition can only by 8192 characters long + ROS_DEBUG("Definition is %i long\n", definition.size()); + CopyString(8192, + definition, + msg.msgDefinition, + "Message definition", + output_topic); + + // Send message + advertisement_info_supplier_->sendEvent(); +} + +void GenericRapidPub::ProcessContent(std::string const& output_topic, + std::string const& md5_sum, + uint8_t const* data, + const size_t data_size, + const int seq_num) { + unsigned int size; + rapid::ext::astrobee::GenericCommsContent &msg = content_supplier_->event(); + + msg.hdr.timeStamp = comms_util::RosTime2RapidTime(ros::Time::now()); + msg.hdr.serial = seq_num; + + // Currently the output topic can only be 128 characters long + CopyString(128, + output_topic, + msg.outputTopic, + "Out topic", + output_topic); + + // Currently the md5 sum can only be 64 characters long + CopyString(64, + md5_sum, + msg.md5Sum, + "Md5 sum", + output_topic); + + // Currently the content can only be 128K bytes long + size = data_size; + if (size > 131071) { + ROS_ERROR("Comms Bridge: The message data for topic %s is greater than 131072 . Please change idl.", + output_topic); + size = 131071; + } + + msg.data.ensure_length(size, (size + 1)); + unsigned char *buf = msg.data.get_contiguous_buffer(); + if (buf == NULL) { + ROS_ERROR("DDS: RTI didn't give a contiguous buffer for the content data!"); + return; + } + std::memset(buf, 0, (size + 1)); + std::memmove(buf, data, size); + + // Send message + content_supplier_->sendEvent(); +} + +} // end namespace ff diff --git a/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp b/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp index 42549fc640..0a2ec0305e 100644 --- a/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp +++ b/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp @@ -18,33 +18,47 @@ #include "comms_bridge/generic_ros_sub_rapid_pub.h" +#include #include +#include +#include namespace ff { -GenericROSSubRapidPub::GenericROSSubRapidPub() : - dds_initialized_(false), advertisement_info_seq_(0) {} +GenericROSSubRapidPub::GenericROSSubRapidPub() : dds_initialized_(false) {} GenericROSSubRapidPub::~GenericROSSubRapidPub() {} -void GenericROSSubRapidPub::InitializeDDS() { - advertisement_info_supplier_.reset( - new GenericROSSubRapidPub::AdvertisementInfoSupplier( - rapid::ext::astrobee::GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC, - "", "AstrobeeGenericCommsAdvertisementInfoProfile", "")); - - content_supplier_.reset(new GenericROSSubRapidPub::ContentSupplier( - rapid::ext::astrobee::GENERIC_COMMS_CONTENT_TOPIC, - "", "AstrobeeGenericCommsContentProfile", "")); +void GenericROSSubRapidPub::AddTopics( + std::map>> const& link_entries) { + std::string in_topic, primary_out_topic; + for (auto it = link_entries.begin(); it != link_entries.end(); ++it) { + in_topic = it->first; + // Use the first out_topic we read in as the out topic the base class uses + primary_out_topic = it->second[0].second; + // Save all robot/out topic pairs so that the bridge can pass the correct + // advertisement info and content message to each roboot that needs it + topic_mapping_[primary_out_topic] = it->second; + // Add topic to base class + ROS_ERROR("Adding topic %s to base class.", in_topic.c_str()); + addTopic(in_topic, primary_out_topic); + } +} - rapid::RapidHelper::initHeader(advertisement_info_supplier_->event().hdr); - rapid::RapidHelper::initHeader(content_supplier_->event().hdr); +void GenericROSSubRapidPub::InitializeDDS(std::vector const& connections) { + // std::string robot_name; + for (size_t i = 0; i < connections.size(); ++i) { + robot_name_ = connections[i]; + GenericRapidPubPtr rapid_pub = std::make_shared(robot_name_); + robot_connections_[robot_name_] = rapid_pub; + } dds_initialized_ = true; } // Called with the mutex held -void GenericROSSubRapidPub::subscribeTopic(std::string const& in_topic, const RelayTopicInfo& info) { +void GenericROSSubRapidPub::subscribeTopic(std::string const& in_topic, + const RelayTopicInfo& info) { // this is just the base subscriber letting us know it's adding a topic // nothing more we need to do } @@ -52,93 +66,38 @@ void GenericROSSubRapidPub::subscribeTopic(std::string const& in_topic, const Re // Called with the mutex held void GenericROSSubRapidPub::advertiseTopic(const RelayTopicInfo& relay_info) { const AdvertisementInfo &info = relay_info.ad_info; - rapid::ext::astrobee::GenericCommsAdvertisementInfo &msg = - advertisement_info_supplier_->event(); - unsigned int size; std::string out_topic = relay_info.out_topic; ROS_ERROR("Received ros advertise topic for topic %s\n", out_topic.c_str()); - msg.hdr.timeStamp = comms_util::RosTime2RapidTime(ros::Time::now()); - msg.hdr.serial = ++advertisement_info_seq_; - - // Currently the output topic can only be 128 characters long - SizeCheck(size, out_topic.size(), 128, "Out topic", out_topic); - std::strncpy(msg.outputTopic, out_topic.data(), size); - msg.outputTopic[size] = '\0'; - - msg.latching = info.latching; - - // Currently the data type can only be 128 characters long - SizeCheck(size, info.data_type.size(), 128, "Data type", out_topic); - std::strncpy(msg.dataType, info.data_type.data(), size); - msg.dataType[size] = '\0'; - - // Currently the md5 sum can only be 64 characters long - SizeCheck(size, info.md5_sum.size(), 64, "MD5 sum", out_topic); - std::strncpy(msg.md5Sum, info.md5_sum.data(), size); - msg.md5Sum[size] = '\0'; - - ROS_ERROR("Defination is %i long\n", info.definition.size()); - // Current the ROS message definition can only be 16384 characters long - SizeCheck(size, info.definition.size(), 16384, "Msg definition", out_topic); - std::strncpy(msg.msgDefinition, info.definition.data(), size); - msg.msgDefinition[size] = '\0'; + // Check robot connection exists + if (robot_connections_.find(robot_name_) == robot_connections_.end()) { + ROS_ERROR("Comms Bridge: No connection for %s.\n", robot_name_.c_str()); + } - // Send message - advertisement_info_supplier_->sendEvent(); + robot_connections_[robot_name_]->ProcessAdvertisementInfo(out_topic, + info.latching, + info.data_type, + info.md5_sum, + info.definition); } // Called with the mutex held void GenericROSSubRapidPub::relayMessage(const RelayTopicInfo& topic_info, ContentInfo const& content_info) { - rapid::ext::astrobee::GenericCommsContent &msg = content_supplier_->event(); - - unsigned int size; std::string out_topic = topic_info.out_topic; ROS_ERROR("Received ros content message for topic %s\n", out_topic.c_str()); - msg.hdr.timeStamp = comms_util::RosTime2RapidTime(ros::Time::now()); - msg.hdr.serial = topic_info.relay_seqnum; - - // Currently the output topic can only be 128 characters long - SizeCheck(size, out_topic.size(), 128, "Out topic", out_topic); - std::strncpy(msg.outputTopic, out_topic.data(), size); - msg.outputTopic[size] = '\0'; - - // Currently the md5 sum can only be 64 characters long - SizeCheck(size, content_info.type_md5_sum.size(), 64, "MD5 sum", out_topic); - std::strncpy(msg.md5Sum, content_info.type_md5_sum.data(), size); - msg.md5Sum[size] = '\0'; - - // Currently the content can only be 128K bytes long - SizeCheck(size, content_info.data_size, 131072, "Data", out_topic); - msg.data.ensure_length(size, (size + 1)); - unsigned char *buf = msg.data.get_contiguous_buffer(); - if (buf == NULL) { - ROS_ERROR("DDS: RTI didn't give a contiguous buffer for the content data!"); - return; + // Check robot connection exists + if (robot_connections_.find(robot_name_) == robot_connections_.end()) { + ROS_ERROR("Comms Bridge: No connection for %s.\n", robot_name_.c_str()); } - std::memset(buf, 0, (size + 1)); - std::memmove(buf, content_info.data, size); - - // Send message - content_supplier_->sendEvent(); -} - -void GenericROSSubRapidPub::SizeCheck(unsigned int &size, - const int size_in, - const int max_size, - std::string const& data_name, - std::string const& topic) { - size = size_in; - // Account for null - if (size > (max_size - 1)) { - ROS_ERROR("Comms Bridge: %s for topic %s is greater than %i characters. Please change idl.", - data_name.c_str(), topic.c_str(), max_size); - size = (max_size - 1); - } + robot_connections_[robot_name_]->ProcessContent(out_topic, + content_info.type_md5_sum, + content_info.data, + content_info.data_size, + topic_info.relay_seqnum); } } // end namespace ff diff --git a/communications/dds_msgs/idl/GenericCommsAdvertisementInfo.idl b/communications/dds_msgs/idl/GenericCommsAdvertisementInfo.idl index 2950f0a493..4267c03a31 100644 --- a/communications/dds_msgs/idl/GenericCommsAdvertisementInfo.idl +++ b/communications/dds_msgs/idl/GenericCommsAdvertisementInfo.idl @@ -7,7 +7,7 @@ module rapid { module ext { module astrobee { - typedef string<16384> String16K; + typedef string<8192> String8K; //@copy-c-declaration class GenericCommsAdvertisementInfoTypeSupport; //@copy-c-declaration class GenericCommsAdvertisementInfoDataWriter; @@ -39,7 +39,7 @@ module rapid { public String64 md5Sum; //@copy-declaration /** ROS message definition */ - public String16K msgDefinition; + public String8K msgDefinition; }; }; }; From ff272da5afc5b936fa9d50220ff6d289881a8a8a Mon Sep 17 00:00:00 2001 From: Kathryn Hamilton Date: Wed, 27 Dec 2023 11:12:15 -0800 Subject: [PATCH 19/32] Progress update. Added an out topic to link entries so one can specify an out topic name if the8y want. Also added code so that all topics don't go to all robots. --- .../config/communications/comms_bridge.config | 9 ++- .../comms_bridge/generic_ros_sub_rapid_pub.h | 7 +- .../comms_bridge/src/comms_bridge_nodelet.cpp | 65 +++++++-------- .../src/generic_ros_sub_rapid_pub.cpp | 80 +++++++++++++------ 4 files changed, 99 insertions(+), 62 deletions(-) diff --git a/astrobee/config/communications/comms_bridge.config b/astrobee/config/communications/comms_bridge.config index 0261712519..68a9e8db0a 100644 --- a/astrobee/config/communications/comms_bridge.config +++ b/astrobee/config/communications/comms_bridge.config @@ -38,13 +38,16 @@ links = { -- and relay_both (to be relayed in both directions). Providing all three fields gives the user -- full directional control while minimizing repetition and copy/paste errors. relay_forward = { - {name = "mgt/executive/agent_state"}, + {in_topic = "beh/inspection/feedback"}, + {in_topic = "beh/inspection/result"}, + {in_topic = "beh/inspection/state"}, }, relay_backward = { - {name = "dummy1"}, + {in_topic = "bumble/beh/inspection/cancel", out_topic = "beh/inspection/cancel"}, + {in_topic = "bumble/beh/inspection/goal", out_topic = "beh/inspection/goal"}, }, relay_both = { - {name = "dummy2"}, + {in_topic = "mgt/executive/agent_state"}, }, } } diff --git a/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h b/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h index c0cc6db79e..d5bef5e3d8 100644 --- a/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h +++ b/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h @@ -47,20 +47,21 @@ class GenericROSSubRapidPub : public BridgeSubscriber { void InitializeDDS(std::vector const& connections); // Called with the mutex held - virtual void subscribeTopic(std::string const& in_topic, const RelayTopicInfo& info); + virtual void subscribeTopic(std::string const& in_topic, + const RelayTopicInfo& info); // Called with the mutex held virtual void advertiseTopic(const RelayTopicInfo& info); // Called with the mutex held - virtual void relayMessage(const RelayTopicInfo& topic_info, ContentInfo const& content_info); + virtual void relayMessage(const RelayTopicInfo& topic_info, + ContentInfo const& content_info); private: bool dds_initialized_; std::map>> topic_mapping_; std::map robot_connections_; - std::string robot_name_; }; } // end namespace ff diff --git a/communications/comms_bridge/src/comms_bridge_nodelet.cpp b/communications/comms_bridge/src/comms_bridge_nodelet.cpp index dd3a40b6ea..fd7b021d9a 100644 --- a/communications/comms_bridge/src/comms_bridge_nodelet.cpp +++ b/communications/comms_bridge/src/comms_bridge_nodelet.cpp @@ -31,7 +31,9 @@ #include #include +#include #include +#include #include #include "comms_bridge/generic_rapid_msg_ros_pub.h" @@ -222,14 +224,7 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { ros_pub_.get()); content_rapid_subs_.push_back(content_sub); } - - std::string ns = std::string("/") + agent_name_ + "/"; - ns[1] = std::tolower(ns[1]); // namespaces don't start with upper case - - for (size_t i = 0; i < topics_sub_.size(); i++) { - ROS_INFO_STREAM("Initialize DDS topic sub: " << topics_sub_[i]); - ros_sub_.addTopic(topics_sub_[i], (ns + topics_sub_[i])); - } + ros_sub_.AddTopics(link_entries_); } bool ReadParams() { @@ -255,34 +250,38 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { return false; } + std::string ns = std::string("/") + agent_name_ + "/"; + ns[1] = std::tolower(ns[1]); // namespaces don't start with upper case ROS_INFO_STREAM("Read Params numebr of links: " << links.GetSize()); for (int i = 1; i <= links.GetSize(); i++) { if (!links.GetTable(i, &link)) { NODELET_ERROR("Comms Bridge Nodelet: Could read link table row %i", i); continue; } - std::string config_agent; + std::string config_agent, connection_agent; ROS_INFO_STREAM("Link " << i << " from " << link.GetStr("from", &config_agent)); ROS_INFO_STREAM("Link " << i << " to " << link.GetStr("to", &config_agent)); if (link.GetStr("from", &config_agent) && config_agent == agent_name_) { - AddRapidConnections(link, "to"); - AddTableToSubs(link, "relay_forward"); - AddTableToSubs(link, "relay_both"); + if (AddRapidConnections(link, "to", connection_agent)) { + AddTableToSubs(link, "relay_forward", ns, connection_agent); + AddTableToSubs(link, "relay_both", ns, connection_agent); + } } else if (link.GetStr("to", &config_agent) && config_agent == agent_name_) { - AddRapidConnections(link, "from"); - AddTableToSubs(link, "relay_backward"); - AddTableToSubs(link, "relay_both"); + if (AddRapidConnections(link, "from", connection_agent)) { + AddTableToSubs(link, "relay_backward", ns, connection_agent); + AddTableToSubs(link, "relay_both", ns, connection_agent); + } } } return true; } - void AddRapidConnections(config_reader::ConfigReader::Table &link_table, - std::string direction) { - std::string connection; + bool AddRapidConnections(config_reader::ConfigReader::Table &link_table, + std::string const& direction, + std::string &connection) { if (!link_table.GetStr(direction.c_str(), &connection)) { NODELET_ERROR("Comms Bridge Nodelet: %s not specified for one link", direction.c_str()); - return; + return false; } // This should be very quick since we shouldn't have more than 2 connections @@ -296,30 +295,31 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { if (!found) { rapid_connections_.push_back(connection); } + return true; } void AddTableToSubs(config_reader::ConfigReader::Table &link_table, - std::string table_name) { + std::string table_name, + std::string const& current_robot_ns, + std::string const& connection_robot) { config_reader::ConfigReader::Table relay_table, relay_item; - std::string topic_name; + std::string in_topic, out_topic; if (link_table.GetTable(table_name.c_str(), &relay_table)) { for (size_t i = 1; i <= relay_table.GetSize(); i++) { relay_table.GetTable(i, &relay_item); - if (!relay_item.GetStr("name", &topic_name)) { - NODELET_ERROR("Comms Bridge Nodelet: Agent topic name not specified!"); + if (!relay_item.GetStr("in_topic", &in_topic)) { + NODELET_ERROR("Comms Bridge Nodelet: In topic not specified!"); continue; } - bool found = false; - for (size_t i = 0; i < topics_sub_.size() && !found; i++) { - if (topic_name == topics_sub_[i]) { - found = true; - } + if (!relay_item.GetStr("out_topic", &out_topic)) { + out_topic = current_robot_ns + in_topic; } - if (!found) { - topics_sub_.push_back(topic_name); - } + // Save all output topics under the same in topic since we don't want + // to subscribe to the same topic multiple times + link_entries_[in_topic].push_back(std::make_pair(connection_robot, + out_topic)); } } } @@ -332,7 +332,8 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { std::shared_ptr dds_entities_factory_; std::shared_ptr ros_pub_; std::string agent_name_, participant_name_; - std::vector rapid_connections_, topics_sub_; + std::vector rapid_connections_; + std::map>> link_entries_; }; PLUGINLIB_EXPORT_CLASS(comms_bridge::CommsBridgeNodelet, nodelet::Nodelet) diff --git a/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp b/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp index 0a2ec0305e..332cc180a2 100644 --- a/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp +++ b/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp @@ -46,13 +46,14 @@ void GenericROSSubRapidPub::AddTopics( } void GenericROSSubRapidPub::InitializeDDS(std::vector const& connections) { - // std::string robot_name; + std::string robot_name; for (size_t i = 0; i < connections.size(); ++i) { - robot_name_ = connections[i]; - GenericRapidPubPtr rapid_pub = std::make_shared(robot_name_); - robot_connections_[robot_name_] = rapid_pub; + robot_name = connections[i]; + GenericRapidPubPtr rapid_pub = std::make_shared(robot_name); + robot_connections_[robot_name] = rapid_pub; } + dds_initialized_ = true; } @@ -66,38 +67,69 @@ void GenericROSSubRapidPub::subscribeTopic(std::string const& in_topic, // Called with the mutex held void GenericROSSubRapidPub::advertiseTopic(const RelayTopicInfo& relay_info) { const AdvertisementInfo &info = relay_info.ad_info; - std::string out_topic = relay_info.out_topic; + std::string out_topic = relay_info.out_topic, robot_name, robot_out_topic; ROS_ERROR("Received ros advertise topic for topic %s\n", out_topic.c_str()); - // Check robot connection exists - if (robot_connections_.find(robot_name_) == robot_connections_.end()) { - ROS_ERROR("Comms Bridge: No connection for %s.\n", robot_name_.c_str()); + // Make sure we recognize the topic + if (topic_mapping_.find(out_topic) == topic_mapping_.end()) { + ROS_ERROR("Comms Bridge: Output topic %s unknown in advertise topic.\n", + out_topic.c_str()); + return; } - robot_connections_[robot_name_]->ProcessAdvertisementInfo(out_topic, - info.latching, - info.data_type, - info.md5_sum, - info.definition); + for (size_t i = 0; i < topic_mapping_[out_topic].size(); ++i) { + robot_name = topic_mapping_[out_topic][i].first; + robot_out_topic = topic_mapping_[out_topic][i].second; + + ROS_ERROR("Robot name: %s Robot out topic: %s\n", robot_name.c_str(), robot_out_topic.c_str()); + + // Check robot connection exists + if (robot_connections_.find(robot_name) == robot_connections_.end()) { + ROS_ERROR("Comms Bridge: No connection for %s.\n", robot_name.c_str()); + continue; + } + + robot_connections_[robot_name]->ProcessAdvertisementInfo(out_topic, + info.latching, + info.data_type, + info.md5_sum, + info.definition); + } } // Called with the mutex held -void GenericROSSubRapidPub::relayMessage(const RelayTopicInfo& topic_info, ContentInfo const& content_info) { - std::string out_topic = topic_info.out_topic; - +void GenericROSSubRapidPub::relayMessage(const RelayTopicInfo& topic_info, + ContentInfo const& content_info) { + std::string out_topic = topic_info.out_topic, robot_name, robot_out_topic; + unsigned int size; ROS_ERROR("Received ros content message for topic %s\n", out_topic.c_str()); - // Check robot connection exists - if (robot_connections_.find(robot_name_) == robot_connections_.end()) { - ROS_ERROR("Comms Bridge: No connection for %s.\n", robot_name_.c_str()); + // Make sure we recognize the topic + if (topic_mapping_.find(out_topic) == topic_mapping_.end()) { + ROS_ERROR("Comms Bridge: Output topic %s unknown in relay message.\n", + out_topic.c_str()); + return; } - robot_connections_[robot_name_]->ProcessContent(out_topic, - content_info.type_md5_sum, - content_info.data, - content_info.data_size, - topic_info.relay_seqnum); + for (size_t i = 0; i < topic_mapping_[out_topic].size(); ++i) { + robot_name = topic_mapping_[out_topic][i].first; + robot_out_topic = topic_mapping_[out_topic][i].second; + + ROS_ERROR("Robot name: %s Robot out topic: %s\n", robot_name.c_str(), robot_out_topic.c_str()); + + // Check robot connection exists + if (robot_connections_.find(robot_name) == robot_connections_.end()) { + ROS_ERROR("Comms Bridge: No connection for %s.\n", robot_name.c_str()); + continue; + } + + robot_connections_[robot_name]->ProcessContent(out_topic, + content_info.type_md5_sum, + content_info.data, + content_info.data_size, + topic_info.relay_seqnum); + } } } // end namespace ff From 43e39f9f06656dbf780fb279730091f4e7cc6ead Mon Sep 17 00:00:00 2001 From: Kathryn Hamilton Date: Wed, 3 Jan 2024 15:07:02 -0800 Subject: [PATCH 20/32] Fixed publishing content received before advertisement info. --- .../include/comms_bridge/generic_rapid_sub.h | 2 +- .../src/generic_rapid_msg_ros_pub.cpp | 36 +++++++++++-------- .../comms_bridge/src/generic_rapid_pub.cpp | 4 +-- .../src/generic_ros_sub_rapid_pub.cpp | 34 ++++++++++-------- 4 files changed, 44 insertions(+), 32 deletions(-) diff --git a/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h b/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h index 1cfd956015..df80029b34 100644 --- a/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h +++ b/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h @@ -73,7 +73,7 @@ class GenericRapidSub { } void operator() (T const* data) { - ROS_ERROR("Received data for topic %s\n", subscribe_topic_.c_str()); + ROS_DEBUG("Received data for topic %s\n", subscribe_topic_.c_str()); ros_pub_->ConvertData(data); } diff --git a/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp index 3fbcab04f3..896f243f5f 100644 --- a/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp +++ b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp @@ -36,7 +36,7 @@ void GenericRapidMsgRosPub::ConvertData( const std::string output_topic = data->outputTopic; - ROS_ERROR("Comms Bridge Nodelet: Received advertisement message for topic %s\n", + ROS_DEBUG("Comms Bridge Nodelet: Received advertisement message for topic %s\n", output_topic.c_str()); AdvertisementInfo ad_info; @@ -62,28 +62,36 @@ void GenericRapidMsgRosPub::ConvertData( const std::string output_topic = data->outputTopic; - ROS_ERROR("Comms Bridge Nodelet: Received content message for topic %s\n", + ROS_DEBUG("Comms Bridge Nodelet: Received content message for topic %s\n", output_topic.c_str()); std::map::iterator iter = m_relay_topics_.find(output_topic); if (iter == m_relay_topics_.end()) { ROS_ERROR("Comms Bridge Nodelet: Received content for topic %s but never received advertisement info.\n", output_topic.c_str()); - } else { - RelayTopicInfo &topic_info = iter->second; - ContentInfo content_info; - content_info.type_md5_sum = data->md5Sum; + RelayTopicInfo topic_info; + topic_info.out_topic = output_topic; + topic_info.ad_info.md5_sum = data->md5Sum; + iter = m_relay_topics_.emplace(output_topic, topic_info).first; - unsigned char* buf = data->data.get_contiguous_buffer(); - for (size_t i = 0; i < data->data.length(); i++) { - content_info.data.push_back(buf[i]); - } + // TODO(Katie) Do we request advertisement info + // requestAdvertisementInfo(output_topic); + } + + RelayTopicInfo &topic_info = iter->second; + + ContentInfo content_info; + content_info.type_md5_sum = data->md5Sum; - if (!relayMessage(topic_info, content_info)) { - ROS_ERROR("Comms Bridge Nodelet: Error relaying message for topic %s\n", - output_topic.c_str()); - } + unsigned char* buf = data->data.get_contiguous_buffer(); + for (size_t i = 0; i < data->data.length(); i++) { + content_info.data.push_back(buf[i]); + } + + if (!relayMessage(topic_info, content_info)) { + ROS_ERROR("Comms Bridge Nodelet: Error relaying message for topic %s\n", + output_topic.c_str()); } } diff --git a/communications/comms_bridge/src/generic_rapid_pub.cpp b/communications/comms_bridge/src/generic_rapid_pub.cpp index 58895b84af..bdf565a7a5 100644 --- a/communications/comms_bridge/src/generic_rapid_pub.cpp +++ b/communications/comms_bridge/src/generic_rapid_pub.cpp @@ -27,7 +27,7 @@ GenericRapidPub::GenericRapidPub(std::string const& robot_name) : std::string dds_topic_name; dds_topic_name = robot_name + "-" + rapid::ext::astrobee::GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC; - ROS_ERROR("Comms Bridge: DDS Pub DDS advertisement info topic name: %s\n", + ROS_DEBUG("Comms Bridge: DDS Pub DDS advertisement info topic name: %s\n", dds_topic_name.c_str()); advertisement_info_supplier_.reset( new GenericRapidPub::AdvertisementInfoSupplier( @@ -35,7 +35,7 @@ GenericRapidPub::GenericRapidPub(std::string const& robot_name) : dds_topic_name = robot_name + "-" + rapid::ext::astrobee::GENERIC_COMMS_CONTENT_TOPIC; - ROS_ERROR("Comms Bridge: DDS Publisher DDS content topic name: %s\n", + ROS_DEBUG("Comms Bridge: DDS Publisher DDS content topic name: %s\n", dds_topic_name.c_str()); content_supplier_.reset(new GenericRapidPub::ContentSupplier( dds_topic_name, "", "AstrobeeGenericCommsContentProfile", "")); diff --git a/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp b/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp index 332cc180a2..0187ff9850 100644 --- a/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp +++ b/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp @@ -32,16 +32,21 @@ GenericROSSubRapidPub::~GenericROSSubRapidPub() {} void GenericROSSubRapidPub::AddTopics( std::map>> const& link_entries) { std::string in_topic, primary_out_topic; - for (auto it = link_entries.begin(); it != link_entries.end(); ++it) { - in_topic = it->first; - // Use the first out_topic we read in as the out topic the base class uses - primary_out_topic = it->second[0].second; - // Save all robot/out topic pairs so that the bridge can pass the correct - // advertisement info and content message to each roboot that needs it - topic_mapping_[primary_out_topic] = it->second; - // Add topic to base class - ROS_ERROR("Adding topic %s to base class.", in_topic.c_str()); - addTopic(in_topic, primary_out_topic); + // Make sure dds is initialized before adding topics + if (dds_initialized_) { + for (auto it = link_entries.begin(); it != link_entries.end(); ++it) { + in_topic = it->first; + // Use the first out_topic we read in as the out topic the base class uses + primary_out_topic = it->second[0].second; + // Save all robot/out topic pairs so that the bridge can pass the correct + // advertisement info and content message to each roboot that needs it + topic_mapping_[primary_out_topic] = it->second; + // Add topic to base class + ROS_DEBUG("Adding topic %s to base class.", in_topic.c_str()); + addTopic(in_topic, primary_out_topic); + } + } else { + ROS_ERROR("Comms Bridge: Cannot add topics until dds is initialized.\n"); } } @@ -53,7 +58,6 @@ void GenericROSSubRapidPub::InitializeDDS(std::vector const& connec robot_connections_[robot_name] = rapid_pub; } - dds_initialized_ = true; } @@ -69,7 +73,7 @@ void GenericROSSubRapidPub::advertiseTopic(const RelayTopicInfo& relay_info) { const AdvertisementInfo &info = relay_info.ad_info; std::string out_topic = relay_info.out_topic, robot_name, robot_out_topic; - ROS_ERROR("Received ros advertise topic for topic %s\n", out_topic.c_str()); + ROS_DEBUG("Received ros advertise topic for topic %s\n", out_topic.c_str()); // Make sure we recognize the topic if (topic_mapping_.find(out_topic) == topic_mapping_.end()) { @@ -82,7 +86,7 @@ void GenericROSSubRapidPub::advertiseTopic(const RelayTopicInfo& relay_info) { robot_name = topic_mapping_[out_topic][i].first; robot_out_topic = topic_mapping_[out_topic][i].second; - ROS_ERROR("Robot name: %s Robot out topic: %s\n", robot_name.c_str(), robot_out_topic.c_str()); + ROS_DEBUG("Robot name: %s Robot out topic: %s\n", robot_name.c_str(), robot_out_topic.c_str()); // Check robot connection exists if (robot_connections_.find(robot_name) == robot_connections_.end()) { @@ -103,7 +107,7 @@ void GenericROSSubRapidPub::relayMessage(const RelayTopicInfo& topic_info, ContentInfo const& content_info) { std::string out_topic = topic_info.out_topic, robot_name, robot_out_topic; unsigned int size; - ROS_ERROR("Received ros content message for topic %s\n", out_topic.c_str()); + ROS_DEBUG("Received ros content message for topic %s\n", out_topic.c_str()); // Make sure we recognize the topic if (topic_mapping_.find(out_topic) == topic_mapping_.end()) { @@ -116,7 +120,7 @@ void GenericROSSubRapidPub::relayMessage(const RelayTopicInfo& topic_info, robot_name = topic_mapping_[out_topic][i].first; robot_out_topic = topic_mapping_[out_topic][i].second; - ROS_ERROR("Robot name: %s Robot out topic: %s\n", robot_name.c_str(), robot_out_topic.c_str()); + ROS_DEBUG("Robot name: %s Robot out topic: %s\n", robot_name.c_str(), robot_out_topic.c_str()); // Check robot connection exists if (robot_connections_.find(robot_name) == robot_connections_.end()) { From 0d1c82b2f3ae1a441642578ff39aca418901e478 Mon Sep 17 00:00:00 2001 From: Kathryn Hamilton Date: Wed, 3 Jan 2024 16:51:25 -0800 Subject: [PATCH 21/32] Added initialize dds to the comms bridge. The comms bridge was changed to start the dds subscribers and publishers either on start up if specified in the comms bridge configuration file or when the executive calls the enable comms service. This matches how the current astrobee to astrobee bridge works. --- .../config/communications/comms_bridge.config | 5 +-- .../comms_bridge/src/comms_bridge_nodelet.cpp | 37 +++++++++++++++++-- 2 files changed, 34 insertions(+), 8 deletions(-) diff --git a/astrobee/config/communications/comms_bridge.config b/astrobee/config/communications/comms_bridge.config index 68a9e8db0a..6b9c927c94 100644 --- a/astrobee/config/communications/comms_bridge.config +++ b/astrobee/config/communications/comms_bridge.config @@ -19,10 +19,7 @@ require "context" -- Enable DDS communications on start -- -- If false, a trigger call will be needed to start communications -- -started = false - --- Rapid subscribers -- -rapid_sub_names = {"Bumble", "Honey", "Queen", "Bsharp", "Wannabee"} +initialize_dds_on_start = false links = { -- Logically, there could be up to three two-way links between the three robots. In practice, we diff --git a/communications/comms_bridge/src/comms_bridge_nodelet.cpp b/communications/comms_bridge/src/comms_bridge_nodelet.cpp index fd7b021d9a..6830c8a0e7 100644 --- a/communications/comms_bridge/src/comms_bridge_nodelet.cpp +++ b/communications/comms_bridge/src/comms_bridge_nodelet.cpp @@ -42,6 +42,8 @@ #include "dds_msgs/AstrobeeConstants.h" +#include "ff_msgs/ResponseOnly.h" + // SoraCore #include "knDds/DdsSupport.h" #include "knDds/DdsEntitiesFactory.h" @@ -61,7 +63,9 @@ namespace comms_bridge { class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { public: - CommsBridgeNodelet() : ff_util::FreeFlyerNodelet("comms_bridge") {} + CommsBridgeNodelet() : ff_util::FreeFlyerNodelet("comms_bridge"), + dds_initialized_(false), + initialize_dds_on_start_(false) {} virtual ~CommsBridgeNodelet() {} @@ -190,7 +194,23 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { dds_entities_factory_.reset(new kn::DdsEntitiesFactorySvc()); dds_entities_factory_->init(dds_params); - InitializeDDS(); + dds_initialize_srv_ = nh->advertiseService( + SERVICE_COMMUNICATIONS_ENABLE_ASTROBEE_INTERCOMMS, + &CommsBridgeNodelet::StartDDS, + this); + + if (initialize_dds_on_start_) { + InitializeDDS(); + } + } + + bool StartDDS(ff_msgs::ResponseOnly::Request& req, + ff_msgs::ResponseOnly::Response& res) { + if (!dds_initialized_) { + InitializeDDS(); + } + res.success = true; + return true; } void InitializeDDS() { @@ -203,7 +223,7 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { connection = rapid_connections_[i]; dds_topic_name = agent_name_ + "-" + rapid::ext::astrobee::GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC; - ROS_ERROR("Comms Bridge: DDS Sub DDS advertisement info topic name: %s\n", + ROS_DEBUG("Comms Bridge: DDS Sub DDS advertisement info topic name: %s\n", dds_topic_name.c_str()); advertisement_info_sub = std::make_shared>( @@ -215,7 +235,7 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { dds_topic_name = agent_name_ + "-" + rapid::ext::astrobee::GENERIC_COMMS_CONTENT_TOPIC; - ROS_ERROR("Comms Bridge: DDS Sub DDS content topic name: %s\n", + ROS_DEBUG("Comms Bridge: DDS Sub DDS content topic name: %s\n", dds_topic_name.c_str()); content_sub = std::make_shared>( "AstrobeeGenericCommsContentProfile", @@ -225,6 +245,7 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { content_rapid_subs_.push_back(content_sub); } ros_sub_.AddTopics(link_entries_); + dds_initialized_ = true; } bool ReadParams() { @@ -243,6 +264,12 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { ros_sub_.setVerbosity(verbose); ros_pub_->setVerbosity(verbose); + initialize_dds_on_start_ = false; + if (!config_params_.GetBool("initialize_dds_on_start", + &initialize_dds_on_start_)) { + NODELET_ERROR("Comms Bridge Nodelet: Could not read initialize dds on start. Setting to false."); + } + // Load shared topic groups config_reader::ConfigReader::Table links, link; if (!config_params_.GetTable("links", &links)) { @@ -325,6 +352,7 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { } private: + bool initialize_dds_on_start_, dds_initialized_; config_reader::ConfigReader config_params_; ff::GenericROSSubRapidPub ros_sub_; std::vector content_rapid_subs_; @@ -334,6 +362,7 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { std::string agent_name_, participant_name_; std::vector rapid_connections_; std::map>> link_entries_; + ros::ServiceServer dds_initialize_srv_; }; PLUGINLIB_EXPORT_CLASS(comms_bridge::CommsBridgeNodelet, nodelet::Nodelet) From 3543d0277189daa7772c0b9f5ed5bcaf208a89a0 Mon Sep 17 00:00:00 2001 From: Kathryn Hamilton Date: Thu, 11 Jan 2024 14:09:11 -0800 Subject: [PATCH 22/32] Changed comms bridge config and changed debug output to info. --- astrobee/config/communications/comms_bridge.config | 12 ++++++------ .../include/comms_bridge/generic_rapid_sub.h | 2 +- .../comms_bridge/src/comms_bridge_nodelet.cpp | 8 ++++---- .../comms_bridge/src/generic_rapid_msg_ros_pub.cpp | 8 ++++---- .../comms_bridge/src/generic_rapid_pub.cpp | 10 +++++----- .../comms_bridge/src/generic_ros_sub_rapid_pub.cpp | 10 +++++----- 6 files changed, 25 insertions(+), 25 deletions(-) diff --git a/astrobee/config/communications/comms_bridge.config b/astrobee/config/communications/comms_bridge.config index 6b9c927c94..2226a75271 100644 --- a/astrobee/config/communications/comms_bridge.config +++ b/astrobee/config/communications/comms_bridge.config @@ -28,23 +28,23 @@ links = { -- A single link entry has required fields "from" and "to" that specify the robot roles involved -- in the link. from = "Bumble", -- manager - to = "Wannabee", -- actor + to = "Honey", -- actor -- The link entry has three optional fields: relay_forward (messages to be relayed only in the -- `from` to `to` direction), relay_backward (to be relayed only in the `to` to `from` direction), -- and relay_both (to be relayed in both directions). Providing all three fields gives the user -- full directional control while minimizing repetition and copy/paste errors. relay_forward = { - {in_topic = "beh/inspection/feedback"}, - {in_topic = "beh/inspection/result"}, - {in_topic = "beh/inspection/state"}, + {in_topic = "honey/command", out_topic="command"}, }, relay_backward = { - {in_topic = "bumble/beh/inspection/cancel", out_topic = "beh/inspection/cancel"}, - {in_topic = "bumble/beh/inspection/goal", out_topic = "beh/inspection/goal"}, + {in_topic = "mgt/ack"}, + {in_topic = "mob/motion/feedback"}, + {in_topic = "beh/dock/feedback"}, }, relay_both = { {in_topic = "mgt/executive/agent_state"}, + {in_topic = "mgt/sys_monitor/state"}, }, } } diff --git a/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h b/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h index df80029b34..b09cb14ff5 100644 --- a/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h +++ b/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h @@ -73,7 +73,7 @@ class GenericRapidSub { } void operator() (T const* data) { - ROS_DEBUG("Received data for topic %s\n", subscribe_topic_.c_str()); + ROS_INFO("Received data for topic %s\n", subscribe_topic_.c_str()); ros_pub_->ConvertData(data); } diff --git a/communications/comms_bridge/src/comms_bridge_nodelet.cpp b/communications/comms_bridge/src/comms_bridge_nodelet.cpp index 6830c8a0e7..c072a58d74 100644 --- a/communications/comms_bridge/src/comms_bridge_nodelet.cpp +++ b/communications/comms_bridge/src/comms_bridge_nodelet.cpp @@ -223,8 +223,8 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { connection = rapid_connections_[i]; dds_topic_name = agent_name_ + "-" + rapid::ext::astrobee::GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC; - ROS_DEBUG("Comms Bridge: DDS Sub DDS advertisement info topic name: %s\n", - dds_topic_name.c_str()); + ROS_INFO("Comms Bridge: DDS Sub DDS advertisement info topic name: %s\n", + dds_topic_name.c_str()); advertisement_info_sub = std::make_shared>( "AstrobeeGenericCommsAdvertisementInfoProfile", @@ -235,8 +235,8 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { dds_topic_name = agent_name_ + "-" + rapid::ext::astrobee::GENERIC_COMMS_CONTENT_TOPIC; - ROS_DEBUG("Comms Bridge: DDS Sub DDS content topic name: %s\n", - dds_topic_name.c_str()); + ROS_INFO("Comms Bridge: DDS Sub DDS content topic name: %s\n", + dds_topic_name.c_str()); content_sub = std::make_shared>( "AstrobeeGenericCommsContentProfile", dds_topic_name, diff --git a/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp index 896f243f5f..b387378b46 100644 --- a/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp +++ b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp @@ -36,8 +36,8 @@ void GenericRapidMsgRosPub::ConvertData( const std::string output_topic = data->outputTopic; - ROS_DEBUG("Comms Bridge Nodelet: Received advertisement message for topic %s\n", - output_topic.c_str()); + ROS_INFO("Comms Bridge Nodelet: Received advertisement message for topic %s\n", + output_topic.c_str()); AdvertisementInfo ad_info; ad_info.latching = data->latching; @@ -62,8 +62,8 @@ void GenericRapidMsgRosPub::ConvertData( const std::string output_topic = data->outputTopic; - ROS_DEBUG("Comms Bridge Nodelet: Received content message for topic %s\n", - output_topic.c_str()); + ROS_INFO("Comms Bridge Nodelet: Received content message for topic %s\n", + output_topic.c_str()); std::map::iterator iter = m_relay_topics_.find(output_topic); if (iter == m_relay_topics_.end()) { diff --git a/communications/comms_bridge/src/generic_rapid_pub.cpp b/communications/comms_bridge/src/generic_rapid_pub.cpp index bdf565a7a5..66a1ba82da 100644 --- a/communications/comms_bridge/src/generic_rapid_pub.cpp +++ b/communications/comms_bridge/src/generic_rapid_pub.cpp @@ -27,16 +27,16 @@ GenericRapidPub::GenericRapidPub(std::string const& robot_name) : std::string dds_topic_name; dds_topic_name = robot_name + "-" + rapid::ext::astrobee::GENERIC_COMMS_ADVERTISEMENT_INFO_TOPIC; - ROS_DEBUG("Comms Bridge: DDS Pub DDS advertisement info topic name: %s\n", - dds_topic_name.c_str()); + ROS_INFO("Comms Bridge: DDS Pub DDS advertisement info topic name: %s\n", + dds_topic_name.c_str()); advertisement_info_supplier_.reset( new GenericRapidPub::AdvertisementInfoSupplier( dds_topic_name, "", "AstrobeeGenericCommsAdvertisementInfoProfile", "")); dds_topic_name = robot_name + "-" + rapid::ext::astrobee::GENERIC_COMMS_CONTENT_TOPIC; - ROS_DEBUG("Comms Bridge: DDS Publisher DDS content topic name: %s\n", - dds_topic_name.c_str()); + ROS_INFO("Comms Bridge: DDS Publisher DDS content topic name: %s\n", + dds_topic_name.c_str()); content_supplier_.reset(new GenericRapidPub::ContentSupplier( dds_topic_name, "", "AstrobeeGenericCommsContentProfile", "")); @@ -108,7 +108,7 @@ void GenericRapidPub::ProcessAdvertisementInfo(std::string const& output_topic, } // Currently the ROS message definition can only by 8192 characters long - ROS_DEBUG("Definition is %i long\n", definition.size()); + ROS_INFO("Definition is %i long\n", definition.size()); CopyString(8192, definition, msg.msgDefinition, diff --git a/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp b/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp index 0187ff9850..5b14d76b1a 100644 --- a/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp +++ b/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp @@ -42,7 +42,7 @@ void GenericROSSubRapidPub::AddTopics( // advertisement info and content message to each roboot that needs it topic_mapping_[primary_out_topic] = it->second; // Add topic to base class - ROS_DEBUG("Adding topic %s to base class.", in_topic.c_str()); + ROS_INFO("Adding topic %s to base class.", in_topic.c_str()); addTopic(in_topic, primary_out_topic); } } else { @@ -73,7 +73,7 @@ void GenericROSSubRapidPub::advertiseTopic(const RelayTopicInfo& relay_info) { const AdvertisementInfo &info = relay_info.ad_info; std::string out_topic = relay_info.out_topic, robot_name, robot_out_topic; - ROS_DEBUG("Received ros advertise topic for topic %s\n", out_topic.c_str()); + ROS_INFO("Received ros advertise topic for topic %s\n", out_topic.c_str()); // Make sure we recognize the topic if (topic_mapping_.find(out_topic) == topic_mapping_.end()) { @@ -86,7 +86,7 @@ void GenericROSSubRapidPub::advertiseTopic(const RelayTopicInfo& relay_info) { robot_name = topic_mapping_[out_topic][i].first; robot_out_topic = topic_mapping_[out_topic][i].second; - ROS_DEBUG("Robot name: %s Robot out topic: %s\n", robot_name.c_str(), robot_out_topic.c_str()); + ROS_INFO("Robot name: %s Robot out topic: %s\n", robot_name.c_str(), robot_out_topic.c_str()); // Check robot connection exists if (robot_connections_.find(robot_name) == robot_connections_.end()) { @@ -107,7 +107,7 @@ void GenericROSSubRapidPub::relayMessage(const RelayTopicInfo& topic_info, ContentInfo const& content_info) { std::string out_topic = topic_info.out_topic, robot_name, robot_out_topic; unsigned int size; - ROS_DEBUG("Received ros content message for topic %s\n", out_topic.c_str()); + ROS_INFO("Received ros content message for topic %s\n", out_topic.c_str()); // Make sure we recognize the topic if (topic_mapping_.find(out_topic) == topic_mapping_.end()) { @@ -120,7 +120,7 @@ void GenericROSSubRapidPub::relayMessage(const RelayTopicInfo& topic_info, robot_name = topic_mapping_[out_topic][i].first; robot_out_topic = topic_mapping_[out_topic][i].second; - ROS_DEBUG("Robot name: %s Robot out topic: %s\n", robot_name.c_str(), robot_out_topic.c_str()); + ROS_INFO("Robot name: %s Robot out topic: %s\n", robot_name.c_str(), robot_out_topic.c_str()); // Check robot connection exists if (robot_connections_.find(robot_name) == robot_connections_.end()) { From 9c7c0c242581795c63237b4dea7a5372020751f1 Mon Sep 17 00:00:00 2001 From: Kathryn Hamilton Date: Thu, 11 Jan 2024 14:16:50 -0800 Subject: [PATCH 23/32] Removed astrobee to astrobee bridge from launch file. --- astrobee/launch/robot/MLP.launch | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/astrobee/launch/robot/MLP.launch b/astrobee/launch/robot/MLP.launch index 9d31a11d73..37b473170a 100644 --- a/astrobee/launch/robot/MLP.launch +++ b/astrobee/launch/robot/MLP.launch @@ -286,16 +286,6 @@ - From 0584433086c066b94f77139cfdcf72575e584e0e Mon Sep 17 00:00:00 2001 From: Kathryn Hamilton Date: Fri, 12 Jan 2024 10:24:35 -0800 Subject: [PATCH 24/32] Changed printfs to ROS INFO. --- .../comms_bridge/src/bridge_publisher.cpp | 20 +++++++++---------- .../comms_bridge/src/bridge_subscriber.cpp | 2 +- .../src/generic_rapid_msg_ros_pub.cpp | 3 +++ 3 files changed, 14 insertions(+), 11 deletions(-) diff --git a/communications/comms_bridge/src/bridge_publisher.cpp b/communications/comms_bridge/src/bridge_publisher.cpp index 48da8fdeab..e1eabf84f4 100644 --- a/communications/comms_bridge/src/bridge_publisher.cpp +++ b/communications/comms_bridge/src/bridge_publisher.cpp @@ -50,12 +50,12 @@ void BridgePublisher::setVerbosity(unsigned int verbosity) { m_verbose_ = verbos bool BridgePublisher::advertiseTopic(const std::string& output_topic, const AdvertisementInfo& ad_info) { if (m_verbose_) - printf("Advertising for topic %s\n", output_topic.c_str()); + ROS_INFO("Advertising for topic %s\n", output_topic.c_str()); std::map::iterator iter = m_relay_topics_.find(output_topic); if (iter == m_relay_topics_.end()) { if (m_verbose_) - printf(" topic is yet-unknown\n"); + ROS_INFO(" topic is yet-unknown\n"); RelayTopicInfo topic_info; topic_info.out_topic = output_topic; iter = m_relay_topics_.emplace(output_topic, topic_info).first; @@ -133,18 +133,18 @@ bool BridgePublisher::relayContent(RelayTopicInfo& topic_info, const ContentInfo m_n_relayed_++; if (m_verbose_) - printf("Sent message on topic %s\n", topic_info.out_topic.c_str()); + ROS_INFO("Sent message on topic %s\n", topic_info.out_topic.c_str()); return true; } bool BridgePublisher::relayMessage(RelayTopicInfo& topic_info, const ContentInfo& content_info) { if (m_verbose_) - printf("Relaying content message for topic %s\n", topic_info.out_topic.c_str()); + ROS_INFO("Relaying content message for topic %s\n", topic_info.out_topic.c_str()); if (!topic_info.advertised) { if (m_verbose_) - printf(" topic is yet-unknown, enqueuing message for now\n"); + ROS_INFO(" topic is yet-unknown, enqueuing message for now\n"); topic_info.waiting_msgs.emplace(content_info); @@ -164,7 +164,7 @@ bool BridgePublisher::relayMessage(RelayTopicInfo& topic_info, const ContentInfo // so just enqueue messages if (m_verbose_) - printf(" topic is not %s, enqueuing message for now\n", + ROS_INFO(" topic is not %s, enqueuing message for now\n", (!topic_info.advertised ? "yet advertised" : "yet ready")); // limit the number of waiting messages, tossing the oldest ones @@ -174,7 +174,7 @@ bool BridgePublisher::relayMessage(RelayTopicInfo& topic_info, const ContentInfo topic_info.waiting_msgs.emplace(content_info); if (m_verbose_ > 1) - printf(" %zu messages now waiting\n", topic_info.waiting_msgs.size()); + ROS_INFO(" %zu messages now waiting\n", topic_info.waiting_msgs.size()); return true; } @@ -204,13 +204,13 @@ void BridgePublisher::drainThread() { timepoint_t t = entry.first; if (m_verbose_ > 1) - printf( + ROS_INFO( "draining %s in %0.3f seconds\n", entry.second.c_str(), std::chrono::duration((t - std::chrono::steady_clock::now())).count()); std::this_thread::sleep_until(t); if (m_verbose_ > 1) - printf("draining %s now\n", entry.second.c_str()); + ROS_INFO("draining %s now\n", entry.second.c_str()); lk.lock(); @@ -221,7 +221,7 @@ void BridgePublisher::drainThread() { // called with mutex held void BridgePublisher::drainWaitingQueue(RelayTopicInfo& topic_info) { if (m_verbose_ && topic_info.waiting_msgs.size() > 0) - printf("Draining queue of %zu waiting messages\n", topic_info.waiting_msgs.size()); + ROS_INFO("Draining queue of %zu waiting messages\n", topic_info.waiting_msgs.size()); while (topic_info.waiting_msgs.size() > 0) { const ContentInfo old_msg = topic_info.waiting_msgs.front(); relayContent(topic_info, old_msg); diff --git a/communications/comms_bridge/src/bridge_subscriber.cpp b/communications/comms_bridge/src/bridge_subscriber.cpp index 577abdfceb..d35987c1eb 100644 --- a/communications/comms_bridge/src/bridge_subscriber.cpp +++ b/communications/comms_bridge/src/bridge_subscriber.cpp @@ -71,7 +71,7 @@ void BridgeSubscriber::handleRelayedMessage(const ros::MessageEvent::iterator iter = m_relay_topics_.find(topic); if (iter == m_relay_topics_.end()) { - printf("Received message on non-relayed topic %s\n", topic.c_str()); + ROS_INFO("Received message on non-relayed topic %s\n", topic.c_str()); return; } diff --git a/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp index b387378b46..325e86d204 100644 --- a/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp +++ b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp @@ -89,6 +89,9 @@ void GenericRapidMsgRosPub::ConvertData( content_info.data.push_back(buf[i]); } + ROS_INFO("Comms Bridge Nodelet: Calling relay message for topic %s\n", + output_topic.c_str()); + if (!relayMessage(topic_info, content_info)) { ROS_ERROR("Comms Bridge Nodelet: Error relaying message for topic %s\n", output_topic.c_str()); From 49d68fbd89d62dd71306dd1ed87994e8524bf1a8 Mon Sep 17 00:00:00 2001 From: Kathryn Hamilton Date: Fri, 12 Jan 2024 12:53:48 -0800 Subject: [PATCH 25/32] Added comms bridge documentation. --- communications/comms_bridge/README.md | 13 ------- communications/comms_bridge/readme.md | 50 +++++++++++++++++++++++++++ 2 files changed, 50 insertions(+), 13 deletions(-) delete mode 100644 communications/comms_bridge/README.md create mode 100644 communications/comms_bridge/readme.md diff --git a/communications/comms_bridge/README.md b/communications/comms_bridge/README.md deleted file mode 100644 index d13d6f3803..0000000000 --- a/communications/comms_bridge/README.md +++ /dev/null @@ -1,13 +0,0 @@ -\page comms_bridge DDS Generic Comms Bridge - - -This is bidirectional bridge to communicate between different astrobees or ros clients using DDS to relay the topics. Supporting an arbitrary number of in-out topics, relaying of ROS messages from one topic to another, without any specification of the message types, nor even the requirement that the types be available to the nodes at compile-time. - -## Basic architecture - -- The subscriber class (A) subscribes to a specified list of topics -- Upon receipt of the first message on a given forwarded topic, A sends an advertisement message to a metadata "advertisement" topic -- Receiving the metadata advertisement, the republisher class (B) itself advertises the output (republished) topic -- The first and all subsequent messages A receives are serialized and published to a metadata "content" topic -- A delay is implemented in B between its advertising and subsequent republishing of any messages to practically avoid the potential race condition of a subscriber on that end missing the first message as it connects to B only upon seeing the advertisement -- Otherwise, B deserializes all messages received on the "content" metadata topic, looks up the topic in previously received "advertisement" metadata, and republishes the message on the output topic diff --git a/communications/comms_bridge/readme.md b/communications/comms_bridge/readme.md new file mode 100644 index 0000000000..7f8f46d3c0 --- /dev/null +++ b/communications/comms_bridge/readme.md @@ -0,0 +1,50 @@ +\page comms_bridge DDS Generic Comms Bridge + +This is a bidirectional bridge to communicate between different astrobees or ros +clients using DDS to relay the topics. Supporting an arbitrary number of in-out +topics, relaying of ROS messages from one topic to another, without any +specification of the message types, nor even the requirement that the types be +available to the nodes at compile-time. + +## Basic architecture + +- The subscriber class (A) subscribes to a specified list of topics +- Upon receipt of the first message on a given forwarded topic, A sends an advertisement message to a metadata "advertisement" topic +- Receiving the metadata advertisement, the republisher class (B) itself advertises the output (republished) topic +- The first and all subsequent messages A receives are serialized and published to a metadata "content" topic +- A delay is implemented in B between its advertising and subsequent republishing of any messages to practically avoid the potential race condition of a subscriber on that end missing the first message as it connects to B only upon seeing the advertisement +- Otherwise, B deserializes all messages received on the "content" metadata topic, looks up the topic in previously received "advertisement" metadata, and republishes the message on the output topic +- In this implementation, the metadata advertisement info and content are converted to and passed over dds. + +## Enabling DDS Communication + +The generic comms bridge runs by default when the astrobee flight starts up. By +default, dds publishing and subscribing is disabled since astrobee to astrobee +communication is not desired for every activity. There are 2 ways to enable it. +One can either change the 'initialize_dds_on_start' parameter in the comms bridge +config file to true before starting flight software or send an "enable astrobee +intercomms" command to astrobee after the flight software has started. + +## Specifing Topics + +To add topics to be passed between robots, please see the comms bridge config +file. The comments in the config file should explain how to add topics. + +## DDS Specifics + +### Discovery Peers File + +In order to have two or more Astrobees communicate, one must add the ip +addresses to the "NDDS_DISCOVERY_PEERS" file in the "dds_generic_comms" folder. +Be sure to add a semi-colon (;) to the end of every line added. + +### QoS Profile + +By default, DDS properties are used to ensure the advertisement info messages +are reliably deliveried to the republisher class even if the subscriber or +republisher are restarted. Currently, DDS is configured to work if there are +no more than 20 topics being passed between robots. If more topics are required, +please change the message depth in the "RAPID_QOS_PROFILES.xml" file in the +"dds_generic_comms" folder. The depth for the "RapidReliableDurableQos" will +need to be changed for both the "datareader_qos" and the "datawriter_qos". This +should be on or around lines 206 and 218 in the file. From 2fb4fc0a512def066cd6f8dbddc19d2ea5c81b0e Mon Sep 17 00:00:00 2001 From: Kathryn Hamilton Date: Fri, 12 Jan 2024 17:06:29 -0800 Subject: [PATCH 26/32] Fixed error output. --- .../comms_bridge/src/generic_rapid_msg_ros_pub.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp index 325e86d204..3617881825 100644 --- a/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp +++ b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp @@ -50,8 +50,8 @@ void GenericRapidMsgRosPub::ConvertData( ROS_INFO_STREAM("ad_info msgDefinition: " << data->msgDefinition); if (!advertiseTopic(output_topic, ad_info)) { - ROS_ERROR("Comms Bridge Nodelet: Error advertising topic: %s\n", - output_topic.c_str()); + ROS_INFO("Comms Bridge Nodelet: Received more than one advertisement info message for topic: %s\n", + output_topic.c_str()); } } @@ -75,7 +75,7 @@ void GenericRapidMsgRosPub::ConvertData( topic_info.ad_info.md5_sum = data->md5Sum; iter = m_relay_topics_.emplace(output_topic, topic_info).first; - // TODO(Katie) Do we request advertisement info + // TODO(Katie) Working on this in another branch // requestAdvertisementInfo(output_topic); } From a1e2a3a87ebc493cf3f33eeafc02e7c957c52d30 Mon Sep 17 00:00:00 2001 From: Kathryn Hamilton Date: Fri, 12 Jan 2024 17:09:52 -0800 Subject: [PATCH 27/32] Increased DDS reader/writer queue size for the generic messages. --- .../communications/dds_generic_comms/RAPID_QOS_PROFILES.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/astrobee/config/communications/dds_generic_comms/RAPID_QOS_PROFILES.xml b/astrobee/config/communications/dds_generic_comms/RAPID_QOS_PROFILES.xml index 78ddb509a0..4643b2feac 100644 --- a/astrobee/config/communications/dds_generic_comms/RAPID_QOS_PROFILES.xml +++ b/astrobee/config/communications/dds_generic_comms/RAPID_QOS_PROFILES.xml @@ -203,7 +203,7 @@ KEEP_LAST_HISTORY_QOS - 1 + 20 @@ -215,7 +215,7 @@ KEEP_LAST_HISTORY_QOS - 1 + 20 From d2395e438e7745b0f1000b60b273eb5c535582e1 Mon Sep 17 00:00:00 2001 From: Kathryn Hamilton Date: Tue, 16 Jan 2024 12:39:56 -0800 Subject: [PATCH 28/32] Added documentation to comms bridge config file. --- .../config/communications/comms_bridge.config | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/astrobee/config/communications/comms_bridge.config b/astrobee/config/communications/comms_bridge.config index 2226a75271..fac31cccc3 100644 --- a/astrobee/config/communications/comms_bridge.config +++ b/astrobee/config/communications/comms_bridge.config @@ -34,6 +34,21 @@ links = { -- `from` to `to` direction), relay_backward (to be relayed only in the `to` to `from` direction), -- and relay_both (to be relayed in both directions). Providing all three fields gives the user -- full directional control while minimizing repetition and copy/paste errors. + + -- Each topic entry can contain an input topic and an output topic. The + -- in topic is the topic being published on the robot sending the data and + -- must be specified. The optional out topic is the name of the topic a + -- user wants the data published on in the receiving robot. If the + -- out topic is not specified, the comms bridge will set it to be the name + -- of the robot sending the data combined with the in topic name. For + -- instance, if the from robot was Bumble and the to robot was Honey and + -- one of the in topics in the relay forward list was "mgt/ack", then it + -- would be published on Honey on topic "bumble/mgt/ack". + -- Please note that only one unique in topic can exist in the relay forward + -- and relay both lists and the relay backward and relay both lists. It is + -- fine to have the same in topic in the relay forward and relay backward + -- lists. + relay_forward = { {in_topic = "honey/command", out_topic="command"}, }, From 82102874469457a2da605fadb989d9fa256d8258 Mon Sep 17 00:00:00 2001 From: Kathryn Hamilton Date: Tue, 16 Jan 2024 15:29:58 -0800 Subject: [PATCH 29/32] Added comms bridge to configs --- astrobee/config/management/fault_table.config | 3 +++ astrobee/config/management/sys_monitor.config | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/astrobee/config/management/fault_table.config b/astrobee/config/management/fault_table.config index 64ee4cd3ad..aab193af50 100644 --- a/astrobee/config/management/fault_table.config +++ b/astrobee/config/management/fault_table.config @@ -110,6 +110,9 @@ subsystems={ }}, }}, {name="communication", nodes={ + {name="comms_bridge", faults={ + {id=51, warning=false, blocking=false, response=command("noOp"), key="HEARTBEAT_MISSING", description="No Heartbeat from Comms Bridge", heartbeat={timeout_sec=1.1, misses=1.0}}, + }}, {name="dds_ros_bridge", faults={ {id=33, warning=false, blocking=true, response=command("fault"), key="HEARTBEAT_MISSING", description="No Heartbeat form DDS ROS Bridge", heartbeat={timeout_sec=1.1, misses=5.0}}, {id=78, warning=false, blocking=true, response=command("unloadNodelet", "dds_ros_bridge", ""), key="INITIALIZATION_FAILED", description="DDS ROS Bridge Initialization Failed"}, diff --git a/astrobee/config/management/sys_monitor.config b/astrobee/config/management/sys_monitor.config index cb0cc55ec4..050fffb979 100644 --- a/astrobee/config/management/sys_monitor.config +++ b/astrobee/config/management/sys_monitor.config @@ -41,8 +41,8 @@ time_drift_thres_sec = 0.25 nodelet_info={ {name="access_control", manager="mlp_management", type="access_control/AccessControl"}, {name="arm", manager="mlp_arm", type="arm/ArmNodelet"}, - {name="astrobee_astrobee_bridge", manager="mlp_multibridge", type="dds_ros_bridge/AstrobeeAstrobeeBridge"}, {name="choreographer", manager="mlp_mobility", type="choreographer/ChoreographerNodelet"}, + {name="comms_bridge", manager="mlp_multibridge", type="comms_bridge/CommsBridgeNodelet"}, {name="ctl", manager="llp_gnc", type="ctl/ctl"}, {name="data_bagger", manager="mlp_recording", type="data_bagger/DataBagger"}, {name="dds_ros_bridge", manager="mlp_communications", type="dds_ros_bridge/DdsRosBridge"}, From 99567a74ed1c691993f83d7fe464447ec1489450 Mon Sep 17 00:00:00 2001 From: Kathryn Hamilton Date: Tue, 16 Jan 2024 15:48:58 -0800 Subject: [PATCH 30/32] Added link to comms bridge config for lab testing --- .../config/communications/comms_bridge.config | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/astrobee/config/communications/comms_bridge.config b/astrobee/config/communications/comms_bridge.config index fac31cccc3..ffe6a7b8d1 100644 --- a/astrobee/config/communications/comms_bridge.config +++ b/astrobee/config/communications/comms_bridge.config @@ -61,6 +61,23 @@ links = { {in_topic = "mgt/executive/agent_state"}, {in_topic = "mgt/sys_monitor/state"}, }, + }, + { + from = "Bsharp", -- manager + to = "Wannabee", -- actor + + relay_forward = { + {in_topic = "wannabee/command", out_topic="command"}, + }, + relay_backward = { + {in_topic = "mgt/ack"}, + {in_topic = "mob/motion/feedback"}, + {in_topic = "beh/dock/feedback"}, + }, + relay_both = { + {in_topic = "mgt/executive/agent_state"}, + {in_topic = "mgt/sys_monitor/state"}, + } } } From 161426b0852638ca855de300532289d5040ccb3c Mon Sep 17 00:00:00 2001 From: Kathryn Hamilton Date: Wed, 17 Jan 2024 15:50:16 -0800 Subject: [PATCH 31/32] Addressed pr comments. --- .../config/communications/comms_bridge.config | 16 ++++------------ .../comms_bridge/src/bridge_publisher.cpp | 5 +++++ 2 files changed, 9 insertions(+), 12 deletions(-) diff --git a/astrobee/config/communications/comms_bridge.config b/astrobee/config/communications/comms_bridge.config index ffe6a7b8d1..cadc204ef6 100644 --- a/astrobee/config/communications/comms_bridge.config +++ b/astrobee/config/communications/comms_bridge.config @@ -50,16 +50,12 @@ links = { -- lists. relay_forward = { - {in_topic = "honey/command", out_topic="command"}, }, relay_backward = { - {in_topic = "mgt/ack"}, - {in_topic = "mob/motion/feedback"}, - {in_topic = "beh/dock/feedback"}, }, relay_both = { - {in_topic = "mgt/executive/agent_state"}, - {in_topic = "mgt/sys_monitor/state"}, + {in_topic = "gnc/ekf"}, + {in_topic = "gs/data"}, }, }, { @@ -67,16 +63,12 @@ links = { to = "Wannabee", -- actor relay_forward = { - {in_topic = "wannabee/command", out_topic="command"}, }, relay_backward = { - {in_topic = "mgt/ack"}, - {in_topic = "mob/motion/feedback"}, - {in_topic = "beh/dock/feedback"}, }, relay_both = { - {in_topic = "mgt/executive/agent_state"}, - {in_topic = "mgt/sys_monitor/state"}, + {in_topic = "gnc/ekf"}, + {in_topic = "gs/data"}, } } } diff --git a/communications/comms_bridge/src/bridge_publisher.cpp b/communications/comms_bridge/src/bridge_publisher.cpp index e1eabf84f4..61c4dfe743 100644 --- a/communications/comms_bridge/src/bridge_publisher.cpp +++ b/communications/comms_bridge/src/bridge_publisher.cpp @@ -118,6 +118,11 @@ bool BridgePublisher::relayContent(RelayTopicInfo& topic_info, const ContentInfo return false; } + // Output error if inconsistent type info + if (topic_info.ad_info.md5_sum != content_info.type_md5_sum) { + ROS_ERROR("Content md5 sum doesn't match the advertiment info md5 sum. This could cause issues!"); + } + // will always succeed, even if inconsistent type info shifter.morph(topic_info.ad_info.md5_sum, topic_info.ad_info.data_type, topic_info.ad_info.definition, ""); From 82c23458c5723869fcb71340b600373381f85fdc Mon Sep 17 00:00:00 2001 From: Kathryn Hamilton Date: Fri, 19 Jan 2024 13:02:36 -0800 Subject: [PATCH 32/32] Addressed pr comments. --- .../config/communications/comms_bridge.config | 25 ++++++++++++++++--- .../dds_generic_comms/NDDS_DISCOVERY_PEERS | 19 +++----------- communications/comms_bridge/readme.md | 6 +++-- .../comms_bridge/src/comms_bridge_nodelet.cpp | 20 +++++++++++---- 4 files changed, 45 insertions(+), 25 deletions(-) diff --git a/astrobee/config/communications/comms_bridge.config b/astrobee/config/communications/comms_bridge.config index cadc204ef6..5b8c1d1094 100644 --- a/astrobee/config/communications/comms_bridge.config +++ b/astrobee/config/communications/comms_bridge.config @@ -21,12 +21,32 @@ require "context" -- If false, a trigger call will be needed to start communications -- initialize_dds_on_start = false +-- The verbosity is used to control the output of the underlying ros +-- publisher/subscriber. Level 2 is the most verbose and will output all of the +-- output including debug statements. Level 1 will output all output except +-- debug statements and level 0 will only output warnings and errors. Since +-- all output statements go directly to the ros log, it is safest to keep it +-- set to 2. +verbose = 2 + +-- ad2pub_delay is a time amount in seconds. The bridge publisher must receive +-- the adverttisement info message for a topic before it can publish a content +-- message for that topic. If the bridge publisher receives the content message +-- before the advertisement info, it will save the content message if the +-- ad2pub_delay is greater than 0. If the bridge publisher receives the +-- advertisement info message within the ad2pub_delay time, it will process +-- and publish the content message. If the comms bridge seems to be dropping +-- the first few messages, try increasing this value. +ad2pub_delay = 3.0 + links = { -- Logically, there could be up to three two-way links between the three robots. In practice, we -- will probably only have one link. { -- A single link entry has required fields "from" and "to" that specify the robot roles involved -- in the link. + -- This link will only work on the ISS since it uses the names of the + -- robots on the ISS. Please add more links if Queen goes back to the ISS. from = "Bumble", -- manager to = "Honey", -- actor @@ -59,6 +79,8 @@ links = { }, }, { + -- This link will only work in the granite lab since it uses the names of + -- the robots in the granite lab. from = "Bsharp", -- manager to = "Wannabee", -- actor @@ -72,6 +94,3 @@ links = { } } } - -verbose = 2 -ad2pub_delay = 3.0 diff --git a/astrobee/config/communications/dds_generic_comms/NDDS_DISCOVERY_PEERS b/astrobee/config/communications/dds_generic_comms/NDDS_DISCOVERY_PEERS index 0f7f694397..dc71e38323 100644 --- a/astrobee/config/communications/dds_generic_comms/NDDS_DISCOVERY_PEERS +++ b/astrobee/config/communications/dds_generic_comms/NDDS_DISCOVERY_PEERS @@ -1,17 +1,6 @@ -;; NDDS_DISCOVERY_PEERS - Default Discovery Configuration File -;;Multicast builtin.udpv4://239.255.0.1 ; default discovery multicast addr +;; NDDS_DISCOVERY_PEERS - Discovery Configuration File +;; This file is used to configure the dds portion of the comms bridge ; EVERY LINE IN THIS FILE MUST END IN A COMMENT. Even blank lines. ; -;; Shared Memory -;shmem:// ; All 'shmem' transport plugin(s) -;builtin.shmem:// ; The builtin builtin 'shmem' transport plugin - ; at network address FCC0::0 -; -;; Unicast -; -;; Localhost -127.0.0.1 ; A comma can be used a separator -; -;; Robot to communicate with -; -;; Custom entries below this line (Vagrant, etc) +;; Add the IP addresses of the robots to communicate with +127.0.0.1 ; Local host is added by default diff --git a/communications/comms_bridge/readme.md b/communications/comms_bridge/readme.md index 7f8f46d3c0..9f89f2b31c 100644 --- a/communications/comms_bridge/readme.md +++ b/communications/comms_bridge/readme.md @@ -25,7 +25,7 @@ One can either change the 'initialize_dds_on_start' parameter in the comms bridg config file to true before starting flight software or send an "enable astrobee intercomms" command to astrobee after the flight software has started. -## Specifing Topics +## Specifying Topics To add topics to be passed between robots, please see the comms bridge config file. The comments in the config file should explain how to add topics. @@ -47,4 +47,6 @@ no more than 20 topics being passed between robots. If more topics are required, please change the message depth in the "RAPID_QOS_PROFILES.xml" file in the "dds_generic_comms" folder. The depth for the "RapidReliableDurableQos" will need to be changed for both the "datareader_qos" and the "datawriter_qos". This -should be on or around lines 206 and 218 in the file. +should be on or around lines 206 and 218 in the file. You will also need to +change the value check in the "comms_bridge_nodelet.cpp" file on or around +line 307. diff --git a/communications/comms_bridge/src/comms_bridge_nodelet.cpp b/communications/comms_bridge/src/comms_bridge_nodelet.cpp index c072a58d74..47d406f45e 100644 --- a/communications/comms_bridge/src/comms_bridge_nodelet.cpp +++ b/communications/comms_bridge/src/comms_bridge_nodelet.cpp @@ -279,6 +279,7 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { std::string ns = std::string("/") + agent_name_ + "/"; ns[1] = std::tolower(ns[1]); // namespaces don't start with upper case + int num_topics = 0; ROS_INFO_STREAM("Read Params numebr of links: " << links.GetSize()); for (int i = 1; i <= links.GetSize(); i++) { if (!links.GetTable(i, &link)) { @@ -286,19 +287,26 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { continue; } std::string config_agent, connection_agent; + num_topics = 0; ROS_INFO_STREAM("Link " << i << " from " << link.GetStr("from", &config_agent)); ROS_INFO_STREAM("Link " << i << " to " << link.GetStr("to", &config_agent)); if (link.GetStr("from", &config_agent) && config_agent == agent_name_) { if (AddRapidConnections(link, "to", connection_agent)) { - AddTableToSubs(link, "relay_forward", ns, connection_agent); - AddTableToSubs(link, "relay_both", ns, connection_agent); + AddTableToSubs(link, "relay_forward", ns, connection_agent, num_topics); + AddTableToSubs(link, "relay_both", ns, connection_agent, num_topics); } } else if (link.GetStr("to", &config_agent) && config_agent == agent_name_) { if (AddRapidConnections(link, "from", connection_agent)) { - AddTableToSubs(link, "relay_backward", ns, connection_agent); - AddTableToSubs(link, "relay_both", ns, connection_agent); + AddTableToSubs(link, "relay_backward", ns, connection_agent, num_topics); + AddTableToSubs(link, "relay_both", ns, connection_agent, num_topics); } } + + // Check to make sure the number of topics added doesn't exceed the + // the number of messages dds reliably delivers + if (num_topics > 20) { + ROS_ERROR("Comms bridge: Num of added topics is greater than the number of topics dds will reliably deliver."); + } } return true; } @@ -328,10 +336,12 @@ class CommsBridgeNodelet : public ff_util::FreeFlyerNodelet { void AddTableToSubs(config_reader::ConfigReader::Table &link_table, std::string table_name, std::string const& current_robot_ns, - std::string const& connection_robot) { + std::string const& connection_robot, + int &num_topics) { config_reader::ConfigReader::Table relay_table, relay_item; std::string in_topic, out_topic; if (link_table.GetTable(table_name.c_str(), &relay_table)) { + num_topics += relay_table.GetSize(); for (size_t i = 1; i <= relay_table.GetSize(); i++) { relay_table.GetTable(i, &relay_item); if (!relay_item.GetStr("in_topic", &in_topic)) {