Removal of Obelisk only messages on C++ and addition of Joystick Interface

docs/source/development.md

@@ -68,6 +68,11 @@ In a seperate terminal, we can run a ROS stack with:
 obk-launch config_file_path=<config file> device_name=<device>
 ```
 
+Specifically, for the dummy examples this may look like:
+```
+obk-launch config_file_path=dummy_cpp.yaml device_name=onboard
+```
+
 All the documentation for the Obelisk terminal aliases can be found [here](obelisk_terminal_aliases.md).
 
 ## Building and Running C++ Code

docs/source/index.rst

@@ -18,6 +18,7 @@ Documentation for ``obelisk``
    using_obelisk.rst
    visualization.md
    logging.md
+   joystick.rst
    obelisk_terminal_aliases.md
    faq.md
 

docs/source/joystick.rst

@@ -0,0 +1,58 @@
+==================
+Joystick Interface
+==================
+
+Many robots need some form of human interface to either move around or change states. We provide a lightweight interface to the `joy ROS package <https://index.ros.org/p/joy/>`_.
+Specifically, we allow users to launch the joy node from the obelisk launch file.
+
+Example Launch Configuration
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+.. code-block:: yaml
+
+    joystick:
+        on: True
+        pub_topic: /obelisk/dummy/joy
+        sub_topic: /obelisk/dummy/joy_feedback
+        device_id: 0
+        device_name: "my_device_name"
+        deadzone: 0.05
+        autorepeat_rate: 20.0
+        sticky_buttons: False
+        coalesce_interval_ms: 1
+
+
+``device_id`` through ``coalesce_interval_ms`` are passed directly to the arguments in the ROS package, and so you should refer to the joy package documentation for their meaning.
+If these keys are not provided then default values will be used.
+
+The settings are only parsed if ``on`` is set to ``True``. The ``pub_topic`` is the name of the topic where the joystick state is published to.
+The ``sub_topic`` is the name of the topic where the joystick feedback, like rumble, is published.
+
+Connecting the Joystick
+^^^^^^^^^^^^^^^^^^^^^^^
+To use the joystick, joy must be able to see and access the joystick. You can connect through one of two ways: (a) USB, (b) bluetooth.
+
+Connecting via bluetooth is decidedly more complicated than through USB. We are still working on a guide for reliable bluetooth communication,
+and therefore we will focus on the USB connection here.
+
+USB
++++
+You can verify if the controller is connected to your computer by using `https://hardwaretester.com/gamepad <https://hardwaretester.com/gamepad>`_.
+This will only verify that nominally the computer can see the joystick, not that its can be seen within a docker container or that joy can see it. This is a good first step.
+
+We can also verify the controller connection by using ``evtest``::
+
+    sudo apt-get update
+    sudo apt-get install evtest
+    sudo evtest /dev/input/event<number>
+
+where event<number> is the the correct event. You can check the connected inputs by using ``ls /dev/input``.
+
+If you use evtest on the correct event then whenever the joystick is used something will be printed to the screen.
+
+You can run ``ros2 run joy joy_enumerate_devices`` to see what devices are found. If no devices are found and you verified with evtest that the joystick is connected then
+you may need to change the permissions on the joystick::
+
+    sudo chmod 666 /dev/input/event<number>
+
+where once again you change the <number> with the correct number. Now the joystick should connect without issue.

obelisk/cpp/obelisk_cpp/include/obelisk_node.h

@@ -1,6 +1,8 @@
 #pragma once
 #include <any>
 #include <chrono>
+#include <functional>
+#include <string>
 #include <tuple>
 
 #include "rcl_interfaces/msg/parameter_event.hpp"
@@ -68,7 +70,6 @@ namespace obelisk {
          */
         struct ObeliskPublisherInfo {
             std::string ros_param;
-            std::string msg_type;
             std::function<std::shared_ptr<ObeliskPublisherBase>(const std::string& config_str)> creator;
         };
 
@@ -111,7 +112,6 @@ namespace obelisk {
          */
         struct ObeliskSubscriptionInfo {
             std::string ros_param;
-            std::string msg_type;
             std::function<std::shared_ptr<ObeliskSubscriptionBase>(const std::string& config_str)> creator;
         };
 
@@ -126,36 +126,6 @@ namespace obelisk {
             std::function<rclcpp::TimerBase::SharedPtr(const std::string&)> creator;
         };
 
-        // clang-format off
-        // Other internal definitions
-        // Allowed Obelisk message types
-        using ObeliskMsgs =
-            std::tuple<obelisk_control_msgs::msg::PositionSetpoint,
-                    obelisk_control_msgs::msg::PDFeedForward,
-                    obelisk_estimator_msgs::msg::EstimatedState,
-                    obelisk_sensor_msgs::msg::ObkJointEncoders,
-                    obelisk_sensor_msgs::msg::TrueSimState,
-                    obelisk_sensor_msgs::msg::ObkImage,
-                    obelisk_sensor_msgs::msg::ObkImu,
-                    obelisk_sensor_msgs::msg::ObkFramePose,
-                    obelisk_std_msgs::msg::FloatMultiArray,
-                    obelisk_std_msgs::msg::UInt8MultiArray>;
-
-        // Allowed non-obelisk message types
-        using ROSAllowedMsgs = std::tuple<rcl_interfaces::msg::ParameterEvent>;
-
-        inline const std::array<std::string, 5> sensor_message_names = {
-            obelisk_sensor_msgs::msg::ObkJointEncoders::MESSAGE_NAME,
-            obelisk_sensor_msgs::msg::TrueSimState::MESSAGE_NAME,
-            obelisk_sensor_msgs::msg::ObkImage::MESSAGE_NAME,
-            obelisk_sensor_msgs::msg::ObkImu::MESSAGE_NAME,
-            obelisk_sensor_msgs::msg::ObkFramePose::MESSAGE_NAME};
-
-        inline const std::array<std::string, 1> estimator_message_names = {
-            obelisk_estimator_msgs::msg::EstimatedState::MESSAGE_NAME};
-
-        // clang-format on
-
     } // namespace internal
 
     /**
@@ -241,7 +211,6 @@ namespace obelisk {
 
             internal::ObeliskPublisherInfo info;
             info.ros_param = ros_param;
-            info.msg_type  = MessageT::MESSAGE_NAME;
             info.creator   = [this](const std::string& config_str) {
                 auto publisher = this->CreatePublisherFromConfigStr<MessageT>(config_str);
                 return std::make_shared<internal::ObeliskPublisher<MessageT>>(publisher);
@@ -288,7 +257,6 @@ namespace obelisk {
 
             internal::ObeliskSubscriptionInfo info;
             info.ros_param = ros_param;
-            info.msg_type  = MessageT::MESSAGE_NAME;
             info.creator   = [this, callback](const std::string& config_str) {
                 auto subscription = this->CreateSubscriptionFromConfigStr<MessageT>(config_str, std::move(callback));
                 return std::make_shared<internal::ObeliskSubscription<MessageT>>(subscription);
@@ -512,78 +480,11 @@ namespace obelisk {
             }
             timers_.clear();
             registered_timers_.clear();
-
             RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " shutdown.");
             return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
         }
 
       protected:
-        /**
-         * @brief Creates a publisher, but first verifies if it is a Obelisk
-         * allowed message type.
-         *
-         * @param topic_name the topic
-         * @param qos
-         * @param non_obelisk determines if we are allowed to publish non-obelisk
-         * messages
-         * @param options
-         * @return the publisher
-         */
-        template <typename MessageT, typename AllocatorT = std::allocator<void>>
-        std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<MessageT, AllocatorT>>
-        create_publisher(const std::string& topic_name, const rclcpp::QoS& qos, bool non_obelisk = false,
-                         const rclcpp::PublisherOptionsWithAllocator<AllocatorT>& options =
-                             (rclcpp_lifecycle::create_default_publisher_options<AllocatorT>())) {
-            // Check if the message type is valid
-            if (!non_obelisk) {
-                if (!(ValidMessage<MessageT, internal::ObeliskMsgs>::value ||
-                      ValidMessage<MessageT, internal::ROSAllowedMsgs>::value)) {
-                    throw std::runtime_error("Provided message type is not a valid Obelisk message!");
-                }
-            } else {
-                RCLCPP_WARN_STREAM(this->get_logger(), "Creating a publisher that can publish non-Obelisk messages. "
-                                                       "This may cause certain API incompatibilities.");
-            }
-
-            return rclcpp_lifecycle::LifecycleNode::create_publisher<MessageT, AllocatorT>(topic_name, qos, options);
-        }
-
-        /**
-         * @brief Creates a subscriber, but first verifies if it is a Obelisk
-         * allowed message type.
-         *
-         * @param topic_name the topic
-         * @param qos
-         * @param callback the callback function
-         * @param non_obelisk determines if we are allowed to subscribe to
-         * non-obelisk messages. Logs warning if true.
-         * @param options
-         * @return the subscription
-         */
-        template <typename MessageT, typename CallbackT, typename AllocatorT = std::allocator<void>,
-                  typename SubscriptionT          = rclcpp::Subscription<MessageT, AllocatorT>,
-                  typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType>
-        std::shared_ptr<SubscriptionT> create_subscription(
-            const std::string& topic_name, const rclcpp::QoS& qos, CallbackT&& callback, bool non_obelisk = false,
-            const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>& options =
-                rclcpp_lifecycle::create_default_subscription_options<AllocatorT>(),
-            typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = (MessageMemoryStrategyT::create_default())) {
-            // Check if the message type is valid
-            if (!non_obelisk) {
-                if (!(ValidMessage<MessageT, internal::ObeliskMsgs>::value ||
-                      ValidMessage<MessageT, internal::ROSAllowedMsgs>::value)) {
-                    throw std::runtime_error("Provided message type is not a valid Obelisk message!");
-                }
-            } else {
-                RCLCPP_WARN_STREAM(this->get_logger(), "Creating a subscriber that can publish non-Obelisk messages. "
-                                                       "This may cause certain API incompatibilities.");
-            }
-
-            return rclcpp_lifecycle::LifecycleNode::create_subscription<MessageT, CallbackT, AllocatorT, SubscriptionT,
-                                                                        MessageMemoryStrategyT>(
-                topic_name, qos, std::move(callback), options, msg_mem_strat);
-        }
-
         /**
          * @brief Creates all the registered publishers.
          */
@@ -592,7 +493,8 @@ namespace obelisk {
                 const std::string param = this->get_parameter(info.ros_param).as_string();
                 if (param == "") {
                     RCLCPP_WARN_STREAM(this->get_logger(),
-                                       "Registered publisher was not provided a config string! Skipping creation.");
+                                       "The registered publisher <"
+                                           << key << "> was not provided a config string! Skipping creation.");
                 } else {
                     publishers_[key] = info.creator(param);
                 }
@@ -607,7 +509,8 @@ namespace obelisk {
                 const std::string param = this->get_parameter(info.ros_param).as_string();
                 if (param == "") {
                     RCLCPP_WARN_STREAM(this->get_logger(),
-                                       "Registered subscription was not provided a config string! Skipping creation.");
+                                       "The registered subscription <"
+                                           << key << "> was not provided a config string! Skipping creation.");
                 } else {
                     subscriptions_[key] = info.creator(param);
                 }
@@ -646,7 +549,6 @@ namespace obelisk {
             const auto config_map   = ParseConfigStr(config);
             const std::string topic = GetTopic(config_map);
             const int depth         = GetHistoryDepth(config_map);
-            const bool non_obelisk  = !GetIsObeliskMsg(config_map);
 
             rclcpp::CallbackGroup::SharedPtr cbg = nullptr; // default group
             try {
@@ -659,7 +561,7 @@ namespace obelisk {
             options.callback_group = cbg;
 
             // Create the subscriber
-            return create_subscription<MessageT>(topic, depth, std::move(callback), non_obelisk, options);
+            return create_subscription<MessageT>(topic, depth, std::move(callback), options);
         }
 
         /**
@@ -675,9 +577,8 @@ namespace obelisk {
             const auto config_map   = ParseConfigStr(config);
             const std::string topic = GetTopic(config_map);
             const int depth         = GetHistoryDepth(config_map);
-            const bool non_obelisk  = !GetIsObeliskMsg(config_map);
 
-            return create_publisher<MessageT>(topic, depth, non_obelisk);
+            return create_publisher<MessageT>(topic, depth);
         }
 
         /**
@@ -780,25 +681,6 @@ namespace obelisk {
             }
         }
 
-        /**
-         * @brief Parses the configuration string map to see if this is restricted
-         * to only obelisk messages.
-         *
-         * @param config_map the map created by ParseConfigStr.
-         * @return use obelisk messages or not.
-         */
-        bool GetIsObeliskMsg(const std::map<std::string, std::string>& config_map) {
-            try {
-                if (config_map.at("non_obelisk") == "True") {
-                    return false;
-                } else {
-                    return true;
-                }
-            } catch (const std::exception& e) {
-                return DEFAULT_IS_OBK_MSG;
-            }
-        }
-
         /**
          * @brief Parse the configuration string map to get the period of the timer.
          *  Throws an error if there is no period.

obelisk/cpp/obelisk_cpp/include/obelisk_sensor.h

@@ -27,20 +27,6 @@ namespace obelisk {
 
             has_sensor_pub_ = false;
 
-            // TODO (@zolkin): find a better way to do this
-            using internal::sensor_message_names;
-            for (const auto& [key, reg_pub] : registered_publishers_) {
-                const std::string* name_ptr =
-                    std::find(sensor_message_names.begin(), sensor_message_names.end(), reg_pub.msg_type);
-                if (name_ptr != sensor_message_names.end()) {
-                    has_sensor_pub_ = true;
-                }
-            }
-
-            if (!has_sensor_pub_) {
-                throw std::runtime_error("Need a sensor publisher in an Obelisk Sensor Node!");
-            }
-
             return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
         }
 

obelisk/cpp/viz/include/obelisk_viz_robot.h

@@ -69,20 +69,6 @@ namespace obelisk::viz {
             RCLCPP_INFO_STREAM(this->get_logger(), "Successfully parsed URDF file.");
             RCLCPP_INFO_STREAM(this->get_logger(), "Robot name: " << model_.getName());
 
-            // Verify that a subscriber has a estimator message type
-            // TODO (@zolkin): find a better way to do this
-            bool has_estimator_sub = false;
-            using internal::estimator_message_names;
-            const std::string* name_ptr = std::find(estimator_message_names.begin(), estimator_message_names.end(),
-                                                    this->registered_subscriptions_[est_key_].msg_type);
-            if (name_ptr != estimator_message_names.end()) {
-                has_estimator_sub = true;
-            }
-
-            if (!has_estimator_sub) {
-                throw std::runtime_error("Subscription message must be of type ObeliskEstimatorMsg!");
-            }
-
             // Use the original lifecycle create function to avoid the warning, as we know we need non-obelisk here.
             // The user can only configure the topic name and history depth.
             // *** Note: The launch file must re-map the corresponding `robot_state_publisher`

obelisk/cpp/zoo/CMakeLists.txt

@@ -9,6 +9,7 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
 find_package(rcl REQUIRED)
+find_package(sensor_msgs REQUIRED)
 
 set(ZOO_INC "${obelisk_cpp_SOURCE_DIR}/obelisk_cpp/include")
 
@@ -81,4 +82,5 @@ target_link_libraries(Zoo INTERFACE Obelisk::Core)
 
 ament_target_dependencies(Zoo INTERFACE
   rclcpp
-  rclcpp_lifecycle)
+  rclcpp_lifecycle
+  sensor_msgs)