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