@@ -161,9 +161,9 @@ void Unicycle2DIgnition::subscriberCallback(const geometry_msgs::msg::PoseWithCo
161
161
}
162
162
}
163
163
164
- bool Unicycle2DIgnition::setPoseServiceCallback (rclcpp::Service<fuse_msgs::srv::SetPose>::SharedPtr service,
165
- std::shared_ptr<rmw_request_id_t > request_id,
166
- const fuse_msgs::srv::SetPose::Request::SharedPtr req)
164
+ bool Unicycle2DIgnition::setPoseServiceCallback (rclcpp::Service<fuse_msgs::srv::SetPose>::SharedPtr const & service,
165
+ std::shared_ptr<rmw_request_id_t > const & request_id,
166
+ fuse_msgs::srv::SetPose::Request::SharedPtr const & req)
167
167
{
168
168
try
169
169
{
@@ -185,8 +185,9 @@ bool Unicycle2DIgnition::setPoseServiceCallback(rclcpp::Service<fuse_msgs::srv::
185
185
}
186
186
187
187
bool Unicycle2DIgnition::setPoseDeprecatedServiceCallback (
188
- rclcpp::Service<fuse_msgs::srv::SetPoseDeprecated>::SharedPtr service, std::shared_ptr<rmw_request_id_t > request_id,
189
- const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req)
188
+ rclcpp::Service<fuse_msgs::srv::SetPoseDeprecated>::SharedPtr const & service,
189
+ std::shared_ptr<rmw_request_id_t > const & request_id,
190
+ fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr const & req)
190
191
{
191
192
try
192
193
{
@@ -205,7 +206,7 @@ bool Unicycle2DIgnition::setPoseDeprecatedServiceCallback(
205
206
}
206
207
207
208
void Unicycle2DIgnition::process (const geometry_msgs::msg::PoseWithCovarianceStamped& pose,
208
- std::function<void ()> post_process)
209
+ std::function<void ()> const & post_process)
209
210
{
210
211
// Verify we are in the correct state to process set pose requests
211
212
if (!started_)
@@ -219,10 +220,10 @@ void Unicycle2DIgnition::process(const geometry_msgs::msg::PoseWithCovarianceSta
219
220
std::to_string (pose.pose .pose .position .x ) + " , " +
220
221
std::to_string (pose.pose .pose .position .y ) + " )." );
221
222
}
222
- auto orientation_norm = std::sqrt (pose.pose .pose .orientation .x * pose.pose .pose .orientation .x +
223
- pose.pose .pose .orientation .y * pose.pose .pose .orientation .y +
224
- pose.pose .pose .orientation .z * pose.pose .pose .orientation .z +
225
- pose.pose .pose .orientation .w * pose.pose .pose .orientation .w );
223
+ auto orientation_norm = std::sqrt (( pose.pose .pose .orientation .x * pose.pose .pose .orientation .x ) +
224
+ ( pose.pose .pose .orientation .y * pose.pose .pose .orientation .y ) +
225
+ ( pose.pose .pose .orientation .z * pose.pose .pose .orientation .z ) +
226
+ ( pose.pose .pose .orientation .w * pose.pose .pose .orientation .w ) );
226
227
if (std::abs (orientation_norm - 1.0 ) > 1.0e-3 )
227
228
{
228
229
throw std::invalid_argument (
0 commit comments