20
20
21
21
#include < limits>
22
22
#include < memory>
23
- #include < rclcpp/logging.hpp>
24
23
#include < string>
25
24
25
+ #include " rclcpp/logging.hpp"
26
26
#include " tf2/utils.hpp"
27
27
#include " tf2_geometry_msgs/tf2_geometry_msgs.hpp"
28
28
@@ -99,21 +99,19 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure(
99
99
}
100
100
catch (const std::exception & e)
101
101
{
102
- fprintf (
103
- stderr,
104
- " Exception thrown during filter chain creation with message : %s \n " ,
105
- e.what ());
102
+ fprintf (stderr, " Exception thrown during filter chain creation with message : %s \n " , e.what ());
106
103
return CallbackReturn::ERROR;
107
104
}
108
105
109
- // As the sensor_filter_chain parameter is of type 'none', we cannot directly check if it is present.
110
- // Even if the sensor_filter_chain parameter is not specified, the filter chain will be correctly
111
- // configured with an empty list of filters (https://github.com/ros/filters/issues/89).
106
+ // As the sensor_filter_chain parameter is of type 'none', we cannot directly check if it is
107
+ // present. Even if the sensor_filter_chain parameter is not specified, the filter chain will be
108
+ // correctly configured with an empty list of filters (https://github.com/ros/filters/issues/89).
112
109
has_filter_chain_ = filter_chain_->configure (
113
110
" sensor_filter_chain" , get_node ()->get_node_logging_interface (),
114
111
get_node ()->get_node_parameters_interface ());
115
112
116
- RCLCPP_INFO_EXPRESSION (get_node ()->get_logger (), has_filter_chain_, " Filter chain is successfully configured!" );
113
+ RCLCPP_INFO_EXPRESSION (
114
+ get_node ()->get_logger (), has_filter_chain_, " Filter chain is successfully configured!" );
117
115
118
116
try
119
117
{
@@ -124,7 +122,7 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure(
124
122
125
123
sensor_filtered_state_publisher_ =
126
124
get_node ()->create_publisher <geometry_msgs::msg::WrenchStamped>(
127
- " ~/wrench_filtered" , rclcpp::SystemDefaultsQoS ());
125
+ " ~/wrench_filtered" , rclcpp::SystemDefaultsQoS ());
128
126
realtime_filtered_publisher_ =
129
127
std::make_unique<StateRTPublisher>(sensor_filtered_state_publisher_);
130
128
}
@@ -143,7 +141,8 @@ controller_interface::CallbackReturn ForceTorqueSensorBroadcaster::on_configure(
143
141
realtime_raw_publisher_->msg_ .header .frame_id = params_.frame_id ;
144
142
realtime_raw_publisher_->unlock ();
145
143
146
- if (has_filter_chain_){
144
+ if (has_filter_chain_)
145
+ {
147
146
realtime_filtered_publisher_->lock ();
148
147
realtime_filtered_publisher_->msg_ .header .frame_id = params_.frame_id ;
149
148
realtime_filtered_publisher_->unlock ();
@@ -201,7 +200,7 @@ controller_interface::return_type ForceTorqueSensorBroadcaster::update_and_write
201
200
realtime_raw_publisher_->unlockAndPublish ();
202
201
}
203
202
204
- if (has_filter_chain_)
203
+ if (has_filter_chain_)
205
204
{
206
205
// Filter sensor data, if no filter chain config was specified, wrench_filtered_ = wrench_raw_
207
206
auto filtered = filter_chain_->update (wrench_raw_, wrench_filtered_);
0 commit comments