diff --git a/astrobee/config/communications/comms_bridge.config b/astrobee/config/communications/comms_bridge.config new file mode 100644 index 0000000000..5b8c1d1094 --- /dev/null +++ b/astrobee/config/communications/comms_bridge.config @@ -0,0 +1,96 @@ +-- 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. + +require "context" + +-- Enable DDS communications on start -- +-- 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 + + -- 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. + + -- 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 = { + }, + relay_backward = { + }, + relay_both = { + {in_topic = "gnc/ekf"}, + {in_topic = "gs/data"}, + }, + }, + { + -- 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 + + relay_forward = { + }, + relay_backward = { + }, + relay_both = { + {in_topic = "gnc/ekf"}, + {in_topic = "gs/data"}, + } + } +} 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..dc71e38323 --- /dev/null +++ b/astrobee/config/communications/dds_generic_comms/NDDS_DISCOVERY_PEERS @@ -0,0 +1,6 @@ +;; 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. +; +;; Add the IP addresses of the robots to communicate with +127.0.0.1 ; Local host is added by default 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..4643b2feac --- /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 + 20 + + + + + RELIABLE_RELIABILITY_QOS + + + TRANSIENT_LOCAL_DURABILITY_QOS + + + KEEP_LAST_HISTORY_QOS + 20 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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/astrobee/config/management/fault_table.config b/astrobee/config/management/fault_table.config index d1f0f7cf1d..aab193af50 100644 --- a/astrobee/config/management/fault_table.config +++ b/astrobee/config/management/fault_table.config @@ -110,8 +110,8 @@ 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="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}}, 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"}, diff --git a/astrobee/launch/robot/MLP.launch b/astrobee/launch/robot/MLP.launch index 096cafee64..37b473170a 100644 --- a/astrobee/launch/robot/MLP.launch +++ b/astrobee/launch/robot/MLP.launch @@ -287,8 +287,8 @@ - - + + diff --git a/communications/comms_bridge/CMakeLists.txt b/communications/comms_bridge/CMakeLists.txt new file mode 100644 index 0000000000..a9f6aaf995 --- /dev/null +++ b/communications/comms_bridge/CMakeLists.txt @@ -0,0 +1,141 @@ +# 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) + +if (USE_DDS) +## Find catkin macros and libraries +find_package(catkin2 REQUIRED COMPONENTS + roscpp + nodelet + message_generation + message_runtime + image_transport + std_msgs + sensor_msgs + config_reader + ff_util + ff_msgs + topic_tools + dds_msgs +) + + +# 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 comms_bridge + CATKIN_DEPENDS message_runtime std_msgs nodelet config_reader ff_util ff_msgs dds_msgs +) + +########### +## Build ## +########### + +# Specify additional locations of header files +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} +) + +file(GLOB cc_files + "src/*.cpp" +) + +# Declare C++ libraries +add_library(comms_bridge + ${cc_files} +) +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} ) + + +############# +## Install ## +############# +# Mark libraries for installation +install(TARGETS ${PROJECT_NAME} + 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} +) + +# Mark launch files for installation +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".svn" EXCLUDE) + +else (USE_DDS) + find_package(catkin REQUIRED COMPONENTS) + catkin_package() +endif (USE_DDS) 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..d8a4202697 --- /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::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_; +}; + +#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..158d85376a --- /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/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..16e46f9c05 --- /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 ConvertData(rapid::ext::astrobee::GenericCommsAdvertisementInfo const* data); + void ConvertData(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_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_rapid_sub.h b/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h new file mode 100644 index 0000000000..b09cb14ff5 --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/generic_rapid_sub.h @@ -0,0 +1,108 @@ +/* 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" + +#include "dds_msgs/GenericCommsAdvertisementInfoSupport.h" +#include "dds_msgs/GenericCommsContentSupport.h" + +namespace ff { + +template +class GenericRapidSub { + public: + GenericRapidSub(const std::string& entity_name, + const std::string& subscribe_topic, + 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; + } + + // start joinable thread + thread_ = std::thread(&GenericRapidSub::ThreadExec, this); + } + + ~GenericRapidSub() { + alive_ = false; // Notify thread to exit + thread_.join(); + } + + void operator() (T const* data) { + ROS_INFO("Received data for topic %s\n", subscribe_topic_.c_str()); + 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_; + + /** + * Function to execute within seperate thread + * process DdsEventLoop at 10Hz + */ + void ThreadExec() { + while (alive_) { + // process events at 10hz + dds_event_loop_.processEvents(kn::milliseconds(100)); + } + } +}; + +typedef std::shared_ptr> + AdvertisementInfoRapidSubPtr; +typedef std::shared_ptr> + ContentRapidSubPtr; + +} // end namespace ff + +#endif // COMMS_BRIDGE_GENERIC_RAPID_SUB_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 new file mode 100644 index 0000000000..d5bef5e3d8 --- /dev/null +++ b/communications/comms_bridge/include/comms_bridge/generic_ros_sub_rapid_pub.h @@ -0,0 +1,69 @@ +/* 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_RAPID_PUB_H_ +#define COMMS_BRIDGE_GENERIC_ROS_SUB_RAPID_PUB_H_ + +/* This is a specialization of ROSBridgeSubscriber using a DDS conduit +*/ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace ff { + +class GenericROSSubRapidPub : public BridgeSubscriber { + public: + GenericROSSubRapidPub(); + ~GenericROSSubRapidPub(); + + void AddTopics(std::map>> const& link_entries); + + void InitializeDDS(std::vector const& connections); + + // 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_; + + std::map>> topic_mapping_; + std::map robot_connections_; +}; + +} // end namespace ff + +#endif // COMMS_BRIDGE_GENERIC_ROS_SUB_RAPID_PUB_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/launch/comms_bridge.launch b/communications/comms_bridge/launch/comms_bridge.launch new file mode 100644 index 0000000000..07ec93abbf --- /dev/null +++ b/communications/comms_bridge/launch/comms_bridge.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/communications/comms_bridge/nodelet_plugins.xml b/communications/comms_bridge/nodelet_plugins.xml new file mode 100644 index 0000000000..f30810f612 --- /dev/null +++ b/communications/comms_bridge/nodelet_plugins.xml @@ -0,0 +1,8 @@ + + + + Nodelet for the comms bridge + + + diff --git a/communications/comms_bridge/package.xml b/communications/comms_bridge/package.xml new file mode 100644 index 0000000000..df3076380b --- /dev/null +++ b/communications/comms_bridge/package.xml @@ -0,0 +1,45 @@ + + + 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_util + ff_msgs + topic_tools + config_reader + dds_msgs + roscpp + nodelet + message_generation + message_runtime + image_transport + std_msgs + sensor_msgs + ff_util + ff_msgs + topic_tools + config_reader + dds_msgs + + + + diff --git a/communications/comms_bridge/readme.md b/communications/comms_bridge/readme.md new file mode 100644 index 0000000000..9f89f2b31c --- /dev/null +++ b/communications/comms_bridge/readme.md @@ -0,0 +1,52 @@ +\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. + +## 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. + +## 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. 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/bridge_publisher.cpp b/communications/comms_bridge/src/bridge_publisher.cpp new file mode 100644 index 0000000000..61c4dfe743 --- /dev/null +++ b/communications/comms_bridge/src/bridge_publisher.cpp @@ -0,0 +1,245 @@ +/* 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_.reset(new std::thread(&BridgePublisher::drainThread, this)); +} + +BridgePublisher::~BridgePublisher() { + worker_thread_->join(); +} + +void BridgePublisher::setVerbosity(unsigned int verbosity) { m_verbose_ = verbosity; } + +bool BridgePublisher::advertiseTopic(const std::string& output_topic, const AdvertisementInfo& ad_info) { + if (m_verbose_) + 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_) + 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; + } + + 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; + } + + // 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, ""); + + 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_) + 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_) + ROS_INFO("Relaying content message for topic %s\n", topic_info.out_topic.c_str()); + + if (!topic_info.advertised) { + if (m_verbose_) + ROS_INFO(" 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_) + 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 + 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) + ROS_INFO(" %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_) + ROS_INFO("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) + 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) + ROS_INFO("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) + 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); + 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..d35987c1eb --- /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_) { + ROS_INFO("got data on %s:\n", topic.c_str()); + 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) + ROS_INFO(" def: \"%s\"\n", ptr->getMessageDefinition().c_str()); + + if (m_verbose_ > 2) { + ROS_INFO(" conn header:\n"); + for (ros::M_string::const_iterator iter = connection_header->begin(); + iter != connection_header->end(); + iter++) + ROS_INFO(" %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()) { + ROS_INFO("Received message on non-relayed topic %s\n", topic.c_str()); + return; + } + + RelayTopicInfo &topic_info = iter->second; + + if (!topic_info.advertised) { + if (m_verbose_) + ROS_INFO(" 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) + ROS_INFO(" 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_) + 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_); + + // Enforce that all relays have a unique input topic + 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; + } + + // 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_) + ROS_ERROR("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/comms_bridge_nodelet.cpp b/communications/comms_bridge/src/comms_bridge_nodelet.cpp new file mode 100644 index 0000000000..47d406f45e --- /dev/null +++ b/communications/comms_bridge/src/comms_bridge_nodelet.cpp @@ -0,0 +1,380 @@ +/* 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 +#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" + +#include "ff_msgs/ResponseOnly.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"), + dds_initialized_(false), + initialize_dds_on_start_(false) {} + + 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]); + } + } + ROS_INFO_STREAM("Comms Bridge Nodelet: agent name " << agent_name_); + + 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; + + 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_; + 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); + + 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() { + std::string connection, dds_topic_name; + ff::AdvertisementInfoRapidSubPtr advertisement_info_sub; + ff::ContentRapidSubPtr content_sub; + 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_INFO("Comms Bridge: DDS Sub DDS advertisement info topic name: %s\n", + dds_topic_name.c_str()); + advertisement_info_sub = + std::make_shared>( + "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_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, + connection, + ros_pub_.get()); + content_rapid_subs_.push_back(content_sub); + } + ros_sub_.AddTopics(link_entries_); + dds_initialized_ = true; + } + + 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_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)) { + ROS_FATAL("Comms Bridge Nodelet: Links not specified!"); + return false; + } + + 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)) { + NODELET_ERROR("Comms Bridge Nodelet: Could read link table row %i", i); + 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, 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, 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; + } + + 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 false; + } + + // This should be very quick since we shouldn't have more than 2 connections + bool found = false; + for (size_t i = 0; i < rapid_connections_.size() && !found; i++) { + if (connection == rapid_connections_[i]) { + found = true; + } + } + + if (!found) { + rapid_connections_.push_back(connection); + } + return true; + } + + void AddTableToSubs(config_reader::ConfigReader::Table &link_table, + std::string table_name, + std::string const& current_robot_ns, + 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)) { + NODELET_ERROR("Comms Bridge Nodelet: In topic not specified!"); + continue; + } + + if (!relay_item.GetStr("out_topic", &out_topic)) { + out_topic = current_robot_ns + in_topic; + } + + // 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)); + } + } + } + + private: + bool initialize_dds_on_start_, dds_initialized_; + 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_; + std::vector rapid_connections_; + std::map>> link_entries_; + ros::ServiceServer dds_initialize_srv_; +}; + +PLUGINLIB_EXPORT_CLASS(comms_bridge::CommsBridgeNodelet, nodelet::Nodelet) + +} // namespace comms_bridge 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..3617881825 --- /dev/null +++ b/communications/comms_bridge/src/generic_rapid_msg_ros_pub.cpp @@ -0,0 +1,101 @@ +/* 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() {} + +// Handle Ad Message +void GenericRapidMsgRosPub::ConvertData( + rapid::ext::astrobee::GenericCommsAdvertisementInfo const* data) { + const std::lock_guard lock(m_mutex_); + + const std::string output_topic = data->outputTopic; + + ROS_INFO("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; + 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_INFO("Comms Bridge Nodelet: Received more than one advertisement info message for topic: %s\n", + output_topic.c_str()); + } +} + +// Handle content message +void GenericRapidMsgRosPub::ConvertData( + rapid::ext::astrobee::GenericCommsContent const* data) { + const std::lock_guard lock(m_mutex_); + + const std::string output_topic = data->outputTopic; + + 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()) { + ROS_ERROR("Comms Bridge Nodelet: Received content for topic %s but never received advertisement info.\n", + output_topic.c_str()); + + 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; + + // TODO(Katie) Working on this in another branch + // requestAdvertisementInfo(output_topic); + } + + 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]); + } + + 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()); + } +} + +} // end namespace ff 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..66a1ba82da --- /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_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_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", "")); + + 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_INFO("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 new file mode 100644 index 0000000000..5b14d76b1a --- /dev/null +++ b/communications/comms_bridge/src/generic_ros_sub_rapid_pub.cpp @@ -0,0 +1,139 @@ +/* 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_rapid_pub.h" + +#include +#include +#include +#include + +namespace ff { + +GenericROSSubRapidPub::GenericROSSubRapidPub() : dds_initialized_(false) {} + +GenericROSSubRapidPub::~GenericROSSubRapidPub() {} + +void GenericROSSubRapidPub::AddTopics( + std::map>> const& link_entries) { + std::string 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_INFO("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"); + } +} + +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) { + // 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 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_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()) { + ROS_ERROR("Comms Bridge: Output topic %s unknown in advertise topic.\n", + out_topic.c_str()); + return; + } + + 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_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()) { + 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, robot_name, robot_out_topic; + unsigned int size; + 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()) { + ROS_ERROR("Comms Bridge: Output topic %s unknown in relay message.\n", + out_topic.c_str()); + return; + } + + 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_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()) { + 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 diff --git a/communications/comms_bridge/src/util.cpp b/communications/comms_bridge/src/util.cpp new file mode 100644 index 0000000000..d90f9bd01c --- /dev/null +++ b/communications/comms_bridge/src/util.cpp @@ -0,0 +1,52 @@ +/* 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 + +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 = 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) { + ros_hdr->frame_id = frame_id; + ros_hdr->stamp = comms_util::RapidTime2RosTime(rapid_hdr.timeStamp); +} diff --git a/communications/dds_msgs/idl/AstrobeeConstants.idl b/communications/dds_msgs/idl/AstrobeeConstants.idl index cbe7caaed9..9d022e663e 100644 --- a/communications/dds_msgs/idl/AstrobeeConstants.idl +++ b/communications/dds_msgs/idl/AstrobeeConstants.idl @@ -34,6 +34,8 @@ 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 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..4267c03a31 --- /dev/null +++ b/communications/dds_msgs/idl/GenericCommsAdvertisementInfo.idl @@ -0,0 +1,46 @@ +/* + * Copyright 2023 (c) 2023 Intelligent Robotics Group, NASA ARC + */ + +#include "Message.idl" + +module rapid { + module ext { + module astrobee { + typedef string<8192> String8K; + + //@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 String64 md5Sum; + + //@copy-declaration /** ROS message definition */ + public String8K msgDefinition; + }; + }; + }; +}; diff --git a/communications/dds_msgs/idl/GenericCommsContent.idl b/communications/dds_msgs/idl/GenericCommsContent.idl new file mode 100644 index 0000000000..5ab9cd5a20 --- /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 String64 md5Sum; + + //@copy-declaration /** Serialized content of the message */ + public OctetSequence128K data; + }; + }; + }; +}; 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) diff --git a/communications/ff_msgs/msg/GenericCommsAdvertisementInfo.msg b/communications/ff_msgs/msg/GenericCommsAdvertisementInfo.msg new file mode 100644 index 0000000000..f0d49d208b --- /dev/null +++ b/communications/ff_msgs/msg/GenericCommsAdvertisementInfo.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/GenericCommsContent.msg b/communications/ff_msgs/msg/GenericCommsContent.msg new file mode 100644 index 0000000000..c768922328 --- /dev/null +++ b/communications/ff_msgs/msg/GenericCommsContent.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/GenericCommsReset.msg b/communications/ff_msgs/msg/GenericCommsReset.msg new file mode 100644 index 0000000000..397561bffd --- /dev/null +++ b/communications/ff_msgs/msg/GenericCommsReset.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", 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