diff --git a/pytrollercli/pytrollercli/resource/include/pytroller.hpp.em b/pytrollercli/pytrollercli/resource/include/pytroller.hpp.em index 13fbae3..7f04ca6 100644 --- a/pytrollercli/pytrollercli/resource/include/pytroller.hpp.em +++ b/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 param_listener_; Params params_; - std::vector joint_names_; - std::string interface_name_; - std::vector command_interface_types_; std::unordered_map states_; std::unordered_map commands_; diff --git a/pytrollercli/pytrollercli/resource/package/CMakeLists.txt.em b/pytrollercli/pytrollercli/resource/package/CMakeLists.txt.em index 80dc7a6..6fd36b4 100644 --- a/pytrollercli/pytrollercli/resource/package/CMakeLists.txt.em +++ b/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 diff --git a/pytrollercli/pytrollercli/resource/script/pytroller_logic_impl.py.em b/pytrollercli/pytrollercli/resource/script/pytroller_logic_impl.py.em index ba51b30..a4f6055 100644 --- a/pytrollercli/pytrollercli/resource/script/pytroller_logic_impl.py.em +++ b/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 diff --git a/pytrollercli/pytrollercli/resource/src/pytroller.cpp.em b/pytrollercli/pytrollercli/resource/src/pytroller.cpp.em index 279ffad..095daf3 100644 --- a/pytrollercli/pytrollercli/resource/src/pytroller.cpp.em +++ b/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 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 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 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( + (*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 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; diff --git a/pytrollercli/pytrollercli/resource/src/pytroller_logic.pyx.em b/pytrollercli/pytrollercli/resource/src/pytroller_logic.pyx.em index 0f34a95..feeb069 100644 --- a/pytrollercli/pytrollercli/resource/src/pytroller_logic.pyx.em +++ b/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 diff --git a/pytrollercli/pytrollercli/resource/src/pytroller_parameters.yaml.em b/pytrollercli/pytrollercli/resource/src/pytroller_parameters.yaml.em index 89da59b..b63de27 100644 --- a/pytrollercli/pytrollercli/resource/src/pytroller_parameters.yaml.em +++ b/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" } diff --git a/pytrollercli/pytrollercli/resource/test/test_params.yaml.em b/pytrollercli/pytrollercli/resource/test/test_params.yaml.em index 78d8b4b..439157e 100644 --- a/pytrollercli/pytrollercli/resource/test/test_params.yaml.em +++ b/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"