Merge pull request #281 from ipa-vsp/humble

Sync Humble

Dockerfile

@@ -1,4 +1,4 @@
-FROM ros:galactic-ros-core-focal
+FROM ros:rolling-ros-core
 
 
 RUN apt-get update \
@@ -14,7 +14,7 @@ WORKDIR /home/can_ws/src
 COPY . ros2_canopen
 
 WORKDIR /home/can_ws/
-RUN . /opt/ros/galactic/setup.sh \
+RUN . /opt/ros/rolling/setup.sh \
     && rosdep init && rosdep update \
     && rosdep install --from-paths src --ignore-src -r -y \
     && colcon build \

canopen/package.xml

@@ -5,6 +5,7 @@
   <version>0.2.7</version>
   <description>Meta-package aggregating the ros2_canopen packages and documentation</description>
   <maintainer email="[email protected]">Christoph Hellmann Santos</maintainer>
+  <maintainer email="[email protected]">Vishnuprasad Prachandabhanu</maintainer>
   <license>Apache-2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>

canopen/sphinx/quickstart/examples.rst

@@ -9,15 +9,15 @@ Service Interface
 
 .. code-block:: bash
 
-    ros2 launch canopen_tests ci402_setup.launch.py
+    ros2 launch canopen_tests cia402_setup.launch.py
 
 
 Managed Service Interface
 -------------------------
 
 .. code-block:: bash
 
-    ros2 launch canopen_tests ci402_lifecycle_setup.launch.py
+    ros2 launch canopen_tests cia402_lifecycle_setup.launch.py
 
 ROS2 Control
 ------------

canopen/sphinx/user-guide/configuration.rst

@@ -111,6 +111,7 @@ but come from the lely core library. Below you find a list of possible configura
   reset_all_nodes;	Specifies whether all slaves shall be reset in case of an error event on a mandatory slave (default: false, see bit 4 in object 1F80).
   stop_all_nodes;	Specifies whether all slaves shall be stopped in case of an error event on a mandatory slave (default: false, see bit 6 in object 1F80).
   boot_time;	The timeout for booting mandatory slaves in ms (default: 0, see object 1F89).
+  boot_timeout;	The timeout for booting all slaves in ms (default: 2000ms).
 
 Device Section
 --------------
@@ -151,6 +152,7 @@ device.
   software_version;	The expected software version (default: 0x00000000, see object 1F55).
   configuration_file;	The name of the file containing the configuration (default: "<dcf_path>/<name>.bin" (where <name> is the section name), see object 1F22).
   restore_configuration;	The sub-index of object 1011 to be used when restoring the configuration (default: 0x00).
+  sdo_timeout_ms; The timeout to use for SDO reads/writes to this device. (default: 20ms)
   sdo;	Additional SDO requests to be sent during configuration (see below).
 
 

canopen/sphinx/user-guide/how-to-create-a-configuration.rst

@@ -19,7 +19,7 @@ You can use ``ros2 pkg create`` command to create your package.
 
 .. code-block:: console
 
-  $ ros2 pkg create --dependencies ros2_canopen lely_core_libraries --build-type ament_cmake {package_name}
+  $ ros2 pkg create --dependencies canopen lely_core_libraries --build-type ament_cmake {package_name}
   $ cd {package_name}
   $ rm -rf src
   $ rm -rf include

canopen/sphinx/user-guide/how-to-create-a-robot-system.rst

@@ -7,7 +7,7 @@ leveragin ros2_control.
 
   .. code-block:: console
 
-    $ ros2 pkg create --dependencies ros2_canopen lely_core_libraries --build-type ament_cmake {package_name}
+    $ ros2 pkg create --dependencies canopen lely_core_libraries --build-type ament_cmake {package_name}
     $ cd {package_name}
     $ rm -rf src
     $ rm -rf include
@@ -81,10 +81,10 @@ leveragin ros2_control.
         - {index: 0x607A, sub_index: 0} # target position
 
   nodes:
-    - joint_1:
-        node_id: 2
-    - joint_2:
-        node_id: 3
+    joint_1:
+      node_id: 2
+    joint_2:
+      node_id: 3
 
 5. Copy the ``cia402_slave.eds`` file from the ``canopen_tests/config/robot_control`` package to the ``config/robot_control`` folder.
 
@@ -130,6 +130,7 @@ leveragin ros2_control.
                   [
                       FindPackageShare("canopen_tests"),
                       "urdf",
+                      "robot_controller",
                       "robot_controller.urdf.xacro",
                   ]
               ),

canopen_402_driver/src/command.cpp

@@ -15,6 +15,7 @@
 // along with this program.  If not, see <https://www.gnu.org/licenses/>.
 //
 #include "canopen_402_driver/command.hpp"
+#include <stdexcept>
 using namespace ros2_canopen;
 
 const Command402::TransitionTable Command402::transitions_;

canopen_base_driver/include/canopen_base_driver/lely_driver_bridge.hpp

@@ -315,6 +315,8 @@ class LelyDriverBridge : public canopen::FiberDriver
   uint8_t nodeid;
   std::string name_;
 
+  std::chrono::milliseconds sdo_timeout;
+
   std::function<void()> on_sync_function_;
 
   // void set_sync_function(std::function<void()> on_sync_function)
@@ -396,11 +398,12 @@ class LelyDriverBridge : public canopen::FiberDriver
    * @param [in] id       NodeId to connect to
    * @param [in] eds      EDS file
    * @param [in] bin      BIN file (concise dcf)
+   * @param [in] timeout  Timeout in milliseconds for SDO reads/writes
    *
    */
   LelyDriverBridge(
     ev_exec_t * exec, canopen::AsyncMaster & master, uint8_t id, std::string name, std::string eds,
-    std::string bin)
+    std::string bin, std::chrono::milliseconds timeout = 20ms)
   : FiberDriver(exec, master, id),
     rpdo_queue(new SafeQueue<COData>()),
     emcy_queue(new SafeQueue<COEmcy>())
@@ -417,6 +420,7 @@ class LelyDriverBridge : public canopen::FiberDriver
       dictionary_->readDCF(a, b, bin.c_str());
     }
     pdo_map_ = dictionary_->createPDOMapping();
+    sdo_timeout = timeout;
   }
 
   /**
@@ -476,7 +480,7 @@ class LelyDriverBridge : public canopen::FiberDriver
         this->running = false;
         this->sdo_cond.notify_one();
       },
-      20ms);
+      this->sdo_timeout);
     return prom->get_future();
   }
 
@@ -568,7 +572,7 @@ class LelyDriverBridge : public canopen::FiberDriver
         this->running = false;
         this->sdo_cond.notify_one();
       },
-      20ms);
+      this->sdo_timeout);
     return prom->get_future();
   }
 
@@ -736,7 +740,7 @@ class LelyDriverBridge : public canopen::FiberDriver
         this->running = false;
         this->sdo_cond.notify_one();
       },
-      20ms);
+      this->sdo_timeout);
   }
 
   template <typename T>
@@ -763,7 +767,7 @@ class LelyDriverBridge : public canopen::FiberDriver
         this->running = false;
         this->sdo_cond.notify_one();
       },
-      20ms);
+      this->sdo_timeout);
   }
 
   template <typename T>
@@ -782,7 +786,7 @@ class LelyDriverBridge : public canopen::FiberDriver
     }
     if (!is_tpdo)
     {
-      if (sync_sdo_read_typed<T>(index, subindex, value, std::chrono::milliseconds(20)))
+      if (sync_sdo_read_typed<T>(index, subindex, value, this->sdo_timeout))
       {
         return value;
       }
@@ -839,7 +843,7 @@ class LelyDriverBridge : public canopen::FiberDriver
     }
     else
     {
-      sync_sdo_write_typed(index, subindex, value, std::chrono::milliseconds(20));
+      sync_sdo_write_typed(index, subindex, value, this->sdo_timeout);
     }
   }
 };

canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver.hpp

@@ -44,6 +44,7 @@ class NodeCanopenBaseDriver : public NodeCanopenDriver<NODETYPE>
   std::mutex driver_mutex_;
   std::shared_ptr<ros2_canopen::LelyDriverBridge> lely_driver_;
   uint32_t period_ms_;
+  int sdo_timeout_ms_;
   bool polling_;
 
   // nmt state callback

canopen_base_driver/include/canopen_base_driver/node_interfaces/node_canopen_base_driver_impl.hpp

@@ -35,13 +35,25 @@ void NodeCanopenBaseDriver<NODETYPE>::init(bool called_from_base)
 template <>
 void NodeCanopenBaseDriver<rclcpp_lifecycle::LifecycleNode>::configure(bool called_from_base)
 {
+  try
+  {
+    this->non_transmit_timeout_ =
+      std::chrono::milliseconds(this->config_["non_transmit_timeout"].as<int>());
+  }
+  catch (...)
+  {
+  }
+  RCLCPP_INFO_STREAM(
+    this->node_->get_logger(),
+    "Non transmit timeout" << static_cast<int>(this->non_transmit_timeout_.count()) << "ms");
+
   try
   {
     polling_ = this->config_["polling"].as<bool>();
   }
   catch (...)
   {
-    RCLCPP_ERROR(this->node_->get_logger(), "Could not polling from config, setting to true.");
+    RCLCPP_WARN(this->node_->get_logger(), "Could not polling from config, setting to true.");
     polling_ = true;
   }
   if (polling_)
@@ -52,7 +64,7 @@ void NodeCanopenBaseDriver<rclcpp_lifecycle::LifecycleNode>::configure(bool call
     }
     catch (...)
     {
-      RCLCPP_ERROR(this->node_->get_logger(), "Could not read period from config, setting to 10ms");
+      RCLCPP_WARN(this->node_->get_logger(), "Could not read period from config, setting to 10ms");
       period_ms_ = 10;
     }
   }
@@ -64,7 +76,7 @@ void NodeCanopenBaseDriver<rclcpp_lifecycle::LifecycleNode>::configure(bool call
   }
   catch (...)
   {
-    RCLCPP_ERROR(
+    RCLCPP_WARN(
       this->node_->get_logger(),
       "Could not read enable diagnostics from config, setting to false.");
     diagnostic_enabled_ = false;
@@ -86,17 +98,39 @@ void NodeCanopenBaseDriver<rclcpp_lifecycle::LifecycleNode>::configure(bool call
     diagnostic_updater_ = std::make_shared<diagnostic_updater::Updater>(this->node_);
     diagnostic_updater_->setHardwareID(std::to_string(this->node_id_));
   }
+
+  std::optional<int> sdo_timeout_ms;
+  try
+  {
+    sdo_timeout_ms = std::optional(this->config_["sdo_timeout_ms"].as<int>());
+  }
+  catch (...)
+  {
+  }
+  sdo_timeout_ms_ = sdo_timeout_ms.value_or(20);
 }
 template <>
 void NodeCanopenBaseDriver<rclcpp::Node>::configure(bool called_from_base)
 {
+  try
+  {
+    this->non_transmit_timeout_ =
+      std::chrono::milliseconds(this->config_["non_transmit_timeout"].as<int>());
+  }
+  catch (...)
+  {
+  }
+  RCLCPP_INFO_STREAM(
+    this->node_->get_logger(),
+    "Non transmit timeout" << static_cast<int>(this->non_transmit_timeout_.count()) << "ms");
+
   try
   {
     polling_ = this->config_["polling"].as<bool>();
   }
   catch (...)
   {
-    RCLCPP_ERROR(this->node_->get_logger(), "Could not polling from config, setting to true.");
+    RCLCPP_WARN(this->node_->get_logger(), "Could not polling from config, setting to true.");
     polling_ = true;
   }
   if (polling_)
@@ -107,7 +141,7 @@ void NodeCanopenBaseDriver<rclcpp::Node>::configure(bool called_from_base)
     }
     catch (...)
     {
-      RCLCPP_ERROR(this->node_->get_logger(), "Could not read period from config, setting to 10ms");
+      RCLCPP_WARN(this->node_->get_logger(), "Could not read period from config, setting to 10ms");
       period_ms_ = 10;
     }
   }
@@ -119,7 +153,7 @@ void NodeCanopenBaseDriver<rclcpp::Node>::configure(bool called_from_base)
   }
   catch (...)
   {
-    RCLCPP_ERROR(
+    RCLCPP_WARN(
       this->node_->get_logger(),
       "Could not read enable diagnostics from config, setting to false.");
     diagnostic_enabled_ = false;
@@ -132,7 +166,7 @@ void NodeCanopenBaseDriver<rclcpp::Node>::configure(bool called_from_base)
     }
     catch (...)
     {
-      RCLCPP_ERROR(
+      RCLCPP_WARN(
         this->node_->get_logger(),
         "Could not read diagnostics period from config, setting to 1000ms");
       diagnostic_period_ms_ = 1000;
@@ -141,6 +175,16 @@ void NodeCanopenBaseDriver<rclcpp::Node>::configure(bool called_from_base)
     diagnostic_updater_ = std::make_shared<diagnostic_updater::Updater>(this->node_);
     diagnostic_updater_->setHardwareID(std::to_string(this->node_id_));
   }
+
+  std::optional<int> sdo_timeout_ms;
+  try
+  {
+    sdo_timeout_ms = std::optional(this->config_["sdo_timeout_ms"].as<int>());
+  }
+  catch (...)
+  {
+  }
+  sdo_timeout_ms_ = sdo_timeout_ms.value_or(20);
 }
 
 template <class NODETYPE>
@@ -211,7 +255,7 @@ void NodeCanopenBaseDriver<NODETYPE>::add_to_master()
       std::scoped_lock<std::mutex> lock(this->driver_mutex_);
       this->lely_driver_ = std::make_shared<ros2_canopen::LelyDriverBridge>(
         *(this->exec_), *(this->master_), this->node_id_, this->node_->get_name(), this->eds_,
-        this->bin_);
+        this->bin_, std::chrono::milliseconds(this->sdo_timeout_ms_));
       this->driver_ = std::static_pointer_cast<lely::canopen::BasicDriver>(this->lely_driver_);
       prom->set_value(lely_driver_);
     });

canopen_core/CMakeLists.txt

@@ -107,6 +107,7 @@ ament_target_dependencies(device_container
 target_link_libraries(device_container
   node_canopen_master
   node_canopen_driver
+  yaml-cpp
 )
 
 add_executable(device_container_node

canopen_core/include/canopen_core/node_interfaces/node_canopen_master.hpp

@@ -85,6 +85,7 @@ class NodeCanopenMaster : public NodeCanopenMasterInterface
   std::string master_dcf_;
   std::string master_bin_;
   std::string can_interface_name_;
+  uint32_t timeout_;
 
   std::thread spinner_;
 
@@ -166,7 +167,23 @@ class NodeCanopenMaster : public NodeCanopenMasterInterface
     this->configured_.store(true);
   }
 
-  virtual void configure(bool called_from_base) {}
+  virtual void configure(bool called_from_base)
+  {
+    std::optional<int> timeout;
+    try
+    {
+      timeout = this->config_["boot_timeout"].as<int>();
+    }
+    catch (...)
+    {
+      RCLCPP_WARN(
+        this->node_->get_logger(),
+        "No timeout parameter found in config file. Using default value of 100ms.");
+    }
+    this->timeout_ = timeout.value_or(2000);
+    RCLCPP_INFO_STREAM(
+      this->node_->get_logger(), "Master boot timeout set to " << this->timeout_ << "ms.");
+  }
 
   /**
    * @brief Activate the driver