diff --git a/example_14/description/ros2_control/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro b/example_14/description/ros2_control/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro index 739ffc8e5..5fb4c76bc 100644 --- a/example_14/description/ros2_control/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro +++ b/example_14/description/ros2_control/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro @@ -5,7 +5,7 @@ params="name prefix use_sim:=^|false slowdown:=2.0"> @@ -31,7 +31,7 @@ - + diff --git a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp index 837d3aa2e..05d2f8494 100644 --- a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp +++ b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp @@ -41,9 +41,8 @@ class RRBotActuatorWithoutFeedback : public hardware_interface::ActuatorInterfac hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; @@ -65,9 +64,6 @@ class RRBotActuatorWithoutFeedback : public hardware_interface::ActuatorInterfac double hw_start_sec_; double hw_stop_sec_; - // Store the command for the simulated robot - double hw_joint_command_; - // Fake "mechanical connection" between actuator and sensor using sockets struct sockaddr_in address_; int socket_port_; diff --git a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp index 5644d714d..5d2302d6a 100644 --- a/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp +++ b/example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp @@ -45,8 +45,6 @@ class RRBotSensorPositionFeedback : public hardware_interface::SensorInterface hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; - std::vector export_state_interfaces() override; - hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; @@ -71,7 +69,6 @@ class RRBotSensorPositionFeedback : public hardware_interface::SensorInterface // Store the command for the simulated robot double measured_velocity; // Local variable, but avoid initialization on each read double last_measured_velocity_; - double hw_joint_state_; // Timestamps to calculate position for velocity rclcpp::Clock clock_; diff --git a/example_14/hardware/rrbot_actuator_without_feedback.cpp b/example_14/hardware/rrbot_actuator_without_feedback.cpp index 4608e8a43..e074a3534 100644 --- a/example_14/hardware/rrbot_actuator_without_feedback.cpp +++ b/example_14/hardware/rrbot_actuator_without_feedback.cpp @@ -53,8 +53,6 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_init( socket_port_ = std::stoi(info_.hardware_parameters["example_param_socket_port"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_joint_command_ = std::numeric_limits::quiet_NaN(); - const hardware_interface::ComponentInfo & joint = info_.joints[0]; // RRBotActuatorWithoutFeedback has exactly one command interface and one joint if (joint.command_interfaces.size() != 1) @@ -74,7 +72,7 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_init( return hardware_interface::CallbackReturn::ERROR; } - // START: This part here is for exemplary purposes - Please do not copy to your production code + // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code // Initialize objects for fake mechanical connection sock_ = socket(AF_INET, SOCK_STREAM, 0); if (sock_ < 0) @@ -129,34 +127,29 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_init( RCLCPP_INFO(get_logger(), "Successfully connected to port %d.", socket_port_); } // END: This part here is for exemplary purposes - Please do not copy to your production code - return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_shutdown( +hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - shutdown(sock_, SHUT_RDWR); // shutdown socket + RCLCPP_INFO(get_logger(), "Configuring ...please wait..."); - return hardware_interface::CallbackReturn::SUCCESS; -} + // reset values always when configuring hardware + for (const auto & [name, descr] : joint_command_interfaces_) + { + set_command(name, 0.0); + } -std::vector -RRBotActuatorWithoutFeedback::export_state_interfaces() -{ - std::vector state_interfaces; - return state_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } -std::vector -RRBotActuatorWithoutFeedback::export_command_interfaces() +hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_shutdown( + const rclcpp_lifecycle::State & /*previous_state*/) { - std::vector command_interfaces; - - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &hw_joint_command_)); + shutdown(sock_, SHUT_RDWR); // shutdown socket - return command_interfaces; + return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_activate( @@ -173,9 +166,9 @@ hardware_interface::CallbackReturn RRBotActuatorWithoutFeedback::on_activate( // END: This part here is for exemplary purposes - Please do not copy to your production code // set some default values for joints - if (std::isnan(hw_joint_command_)) + for (const auto & [name, descr] : joint_command_interfaces_) { - hw_joint_command_ = 0; + set_command(name, 0.0); } RCLCPP_INFO(get_logger(), "Successfully activated!"); @@ -212,12 +205,15 @@ hardware_interface::return_type ros2_control_demo_example_14::RRBotActuatorWitho { // START: This part here is for exemplary purposes - Please do not copy to your production code std::stringstream ss; + std::ostringstream data; + ss << "Writing..." << std::endl; ss << std::fixed << std::setprecision(2); - ss << "Writing command: " << hw_joint_command_ << std::endl; - std::ostringstream data; - data << hw_joint_command_; + auto name = info_.joints[0].name + "/" + hardware_interface::HW_IF_VELOCITY; + ss << "Writing command: " << get_command(name) << std::endl; + + data << get_command(name); ss << "Sending data command: " << data.str() << std::endl; RCLCPP_INFO(get_logger(), ss.str().c_str()); diff --git a/example_14/hardware/rrbot_sensor_for_position_feedback.cpp b/example_14/hardware/rrbot_sensor_for_position_feedback.cpp index 50a7393e2..8e1ba59a4 100644 --- a/example_14/hardware/rrbot_sensor_for_position_feedback.cpp +++ b/example_14/hardware/rrbot_sensor_for_position_feedback.cpp @@ -55,8 +55,6 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_init( socket_port_ = std::stoi(info_.hardware_parameters["example_param_socket_port"]); // END: This part here is for exemplary purposes - Please do not copy to your production code - hw_joint_state_ = std::numeric_limits::quiet_NaN(); - const hardware_interface::ComponentInfo & joint = info_.joints[0]; // RRBotSensorPositionFeedback has exactly one state interface and one joint if (joint.state_interfaces.size() != 1) @@ -169,34 +167,16 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_init( return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_shutdown( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - incoming_data_thread_.join(); // stop reading thread - shutdown(sock_, SHUT_RDWR); // shutdown socket - shutdown(obj_socket_, SHUT_RDWR); // shutdown socket - - return hardware_interface::CallbackReturn::SUCCESS; -} - -std::vector -RRBotSensorPositionFeedback::export_state_interfaces() -{ - std::vector state_interfaces; - - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[0].name, hardware_interface::HW_IF_POSITION, &hw_joint_state_)); - - return state_interfaces; -} - hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { + RCLCPP_INFO(get_logger(), "Configuring ...please wait..."); + // set some default values for joints - if (std::isnan(hw_joint_state_)) + // reset values always when configuring hardware + for (const auto & [name, descr] : sensor_state_interfaces_) { - hw_joint_state_ = 0; + set_state(name, 0.0); } last_measured_velocity_ = 0; @@ -207,6 +187,16 @@ hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_configure( return hardware_interface::CallbackReturn::SUCCESS; } +hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_shutdown( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + incoming_data_thread_.join(); // stop reading thread + shutdown(sock_, SHUT_RDWR); // shutdown socket + shutdown(obj_socket_, SHUT_RDWR); // shutdown socket + + return hardware_interface::CallbackReturn::SUCCESS; +} + hardware_interface::CallbackReturn RRBotSensorPositionFeedback::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { @@ -254,17 +244,21 @@ hardware_interface::return_type RRBotSensorPositionFeedback::read( std::stringstream ss; ss << "Reading..." << std::endl; - // Simulate RRBot's movement + // Sensor reading measured_velocity = *(rt_incomming_data_ptr_.readFromRT()); if (!std::isnan(measured_velocity)) { last_measured_velocity_ = measured_velocity; } - hw_joint_state_ += (last_measured_velocity_ * duration.seconds()) / hw_slowdown_; + + // integrate velocity to position + auto name = info_.joints[0].name + "/" + hardware_interface::HW_IF_POSITION; + auto new_value = get_state(name) + (last_measured_velocity_ * duration.seconds()) / hw_slowdown_; + set_state(name, new_value); ss << std::fixed << std::setprecision(2); ss << "Got measured velocity " << measured_velocity << std::endl; - ss << "Got state " << hw_joint_state_ << " for joint '" << info_.joints[0].name << "'" + ss << "Got state(position) " << new_value << " for joint '" << info_.joints[0].name << "'" << std::endl; RCLCPP_INFO(get_logger(), ss.str().c_str()); // END: This part here is for exemplary purposes - Please do not copy to your production code