Merge pull request #3 from ICube-Robotics/tpoignonec-optionnal_cmd_msg

Tpoignonec optionnal cmd msg + other features

pytrollercli/pytrollercli/resource/include/pytroller.hpp.em

@@ -73,16 +73,12 @@ public:
     const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
 protected:
-
   void declare_parameters();
   controller_interface::CallbackReturn read_parameters();
 
   std::shared_ptr<ParamListener> param_listener_;
   Params params_;
 
-  std::vector<std::string> joint_names_;
-  std::string interface_name_;
-
   std::vector<std::string> command_interface_types_;
   std::unordered_map<std::string, double> states_;
   std::unordered_map<std::string, double> commands_;

pytrollercli/pytrollercli/resource/package/CMakeLists.txt.em

@@ -44,6 +44,7 @@ add_custom_command(
   WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/src
   DEPENDS
     ${CMAKE_CURRENT_SOURCE_DIR}/src/@(pytroller_name)_logic.pyx
+    ${CMAKE_CURRENT_SOURCE_DIR}/script/@(pytroller_name)_logic_impl.py
 )
 
 # Copy the header file into the include directory

pytrollercli/pytrollercli/resource/script/pytroller_logic_impl.py.em

@@ -12,6 +12,6 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-def pytroller_logic_impl(states, commands, msg, params):
+def pytroller_logic_impl(period, states, commands, msg, params):
 
   return commands

pytrollercli/pytrollercli/resource/src/pytroller.cpp.em

@@ -88,9 +88,20 @@ controller_interface::CallbackReturn @(pytroller_class)::on_configure(
     states_.insert({state_interfaces_[index].get_name(), state_interfaces_[index].get_value()});
   }
 
-  command_subscriber_ = get_node()->create_generic_subscription(
-    params_.command_topic_name, params_.command_topic_type, rclcpp::SystemDefaultsQoS(),
-    [this](std::shared_ptr<rclcpp::SerializedMessage> msg) { rt_command_ptr_.writeFromNonRT(msg); });
+  if (params_.command_topic_name.empty() != params_.command_topic_type.empty()) {
+    RCLCPP_FATAL(
+      get_node()->get_logger(),
+      "The parameters 'command_topic_name' and 'command_topic_type' should BOTH be empty OR set!");
+    return controller_interface::CallbackReturn::ERROR;
+  }
+  if (!params_.command_topic_name.empty()) {
+    command_subscriber_ = get_node()->create_generic_subscription(
+      params_.command_topic_name, params_.command_topic_type, rclcpp::SystemDefaultsQoS(),
+      [this](std::shared_ptr<rclcpp::SerializedMessage> msg) { rt_command_ptr_.writeFromNonRT(msg); });
+  }
+  else {
+    command_subscriber_ = nullptr;
+  }
 
   RCLCPP_INFO(get_node()->get_logger(), "configure successful");
   return controller_interface::CallbackReturn::SUCCESS;
@@ -145,13 +156,26 @@ controller_interface::CallbackReturn @(pytroller_class)::on_deactivate(
 }
 
 controller_interface::return_type @(pytroller_class)::update(
-  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
+  const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
 {
-  auto msg = rt_command_ptr_.readFromRT();
+  // update parameters if changed
+  if (param_listener_->is_old(params_)) {
+    params_ = param_listener_->get_params();
+  }
 
-  // no command received yet
-  if (!msg || !(*msg)) {
-    return controller_interface::return_type::OK;
+  // Read command msg if needed
+  std::vector<int> message;
+  if (command_subscriber_) {
+    auto msg = rt_command_ptr_.readFromRT();
+    if (!msg || !(*msg)) {
+      // no command received yet
+      return controller_interface::return_type::OK;
+    }
+    // fill message buffer to be passed to python
+    message = std::vector<int>(
+      (*msg)->get_rcl_serialized_message().buffer,
+      (*msg)->get_rcl_serialized_message().buffer +  (*msg)->size()
+    );
   }
 
   // fill commands and states maps to be passed to python
@@ -162,17 +186,8 @@ controller_interface::return_type @(pytroller_class)::update(
     states_[state_interfaces_[index].get_name()] = state_interfaces_[index].get_value();
   }
 
-  // update parameters if changed
-  if (param_listener_->is_old(params_)) {
-    params_ = param_listener_->get_params();
-  }
-
-  // fill message buffer to be passed to python
-  std::vector<int> message((*msg)->get_rcl_serialized_message().buffer,
-    (*msg)->get_rcl_serialized_message().buffer +  (*msg)->size());
-
   // run cython function
-  if (@(pytroller_name)_logic(states_, commands_, message, params_)) {
+  if (@(pytroller_name)_logic(period.seconds(), states_, commands_, message, params_)) {
     RCLCPP_ERROR_THROTTLE(
       get_node()->get_logger(), *(get_node()->get_clock()), 1000,
       "@(pytroller_name) logic failed.");
@@ -201,21 +216,14 @@ controller_interface::CallbackReturn @(pytroller_class)::read_parameters()
   }
   params_ = param_listener_->get_params();
 
-  if (params_.joints.empty())
+  if (params_.interface_full_names.empty())
   {
-    RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty");
+    RCLCPP_ERROR(get_node()->get_logger(), "'interface_full_names' parameter was empty");
     return controller_interface::CallbackReturn::ERROR;
   }
 
-  if (params_.interface_name.empty())
-  {
-    RCLCPP_ERROR(get_node()->get_logger(), "'interface_name' parameter was empty");
-    return controller_interface::CallbackReturn::ERROR;
-  }
-
-  for (const auto & joint : params_.joints)
-  {
-    command_interface_types_.push_back(joint + "/" + params_.interface_name);
+  for (const auto & interface_name : params_.interface_full_names) {
+    command_interface_types_.push_back(interface_name);
   }
 
   return controller_interface::CallbackReturn::SUCCESS;

pytrollercli/pytrollercli/resource/src/pytroller_logic.pyx.em

@@ -29,13 +29,22 @@ include "@(pytroller_name)_parameters.pxd"
 import importlib
 from rclpy.serialization import deserialize_message
 
-cdef public int @(pytroller_name)_logic(unordered_map[string, double] states, unordered_map[string, double] & commands, vector[int] & msg, Params param):
+cdef public int @(pytroller_name)_logic(
+  double period,
+  unordered_map[string, double] states,
+  unordered_map[string, double] & commands,
+  vector[int] & msg,
+  Params param
+):
   try:
-    mt = param.command_topic_type.decode("utf-8").split('/')
-    messagetype = getattr(importlib.import_module('.'.join(mt[:2])), mt[-1])
-    command_message = deserialize_message(bytes(msg), type(messagetype()))
-
-    (&commands)[0] = pytroller_logic_impl(states, commands, command_message, param)
+    command_message = None
+    # Read command msg if applicable (i.e., there is one...)
+    if (len(msg) > 0):
+      mt = param.command_topic_type.decode("utf-8").split('/')
+      messagetype = getattr(importlib.import_module('.'.join(mt[:2])), mt[-1])
+      command_message = deserialize_message(bytes(msg), type(messagetype()))
+    # Either way, call python logic
+    (&commands)[0] = pytroller_logic_impl(period, states, commands, command_message, param)
   except Exception as error:
     print("An exception occurred:", error)
     return -1

pytrollercli/pytrollercli/resource/src/pytroller_parameters.yaml.em

@@ -1,21 +1,16 @@
 @(pytroller_name):
-  joints: {
+  interface_full_names: {
     type: string_array,
     default_value: [],
-    description: "Name of the joints to control",
-  }
-  interface_name: {
-    type: string,
-    default_value: "",
-    description: "Name of the interface to command",
+    description: "Name (WARNING, full names, e.g., 'joint_1/effort') of the interface(s) to command",
   }
   command_topic_name: {
     type: string,
-    default_value: "~/commands",
-    description: "Name of the subscribed command topic"
+    default_value: "",
+    description: "Optional. Name of the subscribed command topic"
   }
   command_topic_type: {
     type: string,
-    default_value: "std_msgs/msg/Float64MultiArray",
-    description: "Type of the subscribed command topic"
+    default_value: "",
+    description: "Optional. Type of the subscribed command topic"
   }

pytrollercli/pytrollercli/resource/test/test_params.yaml.em

@@ -1,13 +1,15 @@
 load_@(pytroller_name):
   ros__parameters:
-    joints: ["joint1", "joint2"]
-    interface_name: "position"
+    interface_full_names:
+     - "joint1/position"
+     - "joint2/position"
     command_topic_name: "~/commands"
     command_topic_type: "std_msgs/msg/Float64MultiArray"
 
 test_@(pytroller_name):
   ros__parameters:
-    joints: ["joint1", "joint2"]
-    interface_name: "position"
+    interface_full_names:
+     - "joint1/position"
+     - "joint2/position"
     command_topic_name: "~/commands"
     command_topic_type: "std_msgs/msg/Float64MultiArray"