@@ -23,8 +23,8 @@ along with this program; or you can read the full license at
2323
2424#include " urn_jaus_jss_iop_CostMap2DClient/CostMap2DClient_ReceiveFSM.h"
2525#include < fkie_iop_component/iop_config.hpp>
26- #include < tf2/LinearMath/Quaternion.h >
27- #include < tf2/LinearMath/Transform.h >
26+ #include < tf2/LinearMath/Quaternion.hpp >
27+ #include < tf2/LinearMath/Transform.hpp >
2828
2929#include < fkie_iop_builder/util.h>
3030
@@ -39,8 +39,7 @@ namespace urn_jaus_jss_iop_CostMap2DClient
3939
4040CostMap2DClient_ReceiveFSM::CostMap2DClient_ReceiveFSM (std::shared_ptr<iop::Component> cmp, urn_jaus_jss_core_AccessControlClient::AccessControlClient_ReceiveFSM* pAccessControlClient_ReceiveFSM, urn_jaus_jss_core_EventsClient::EventsClient_ReceiveFSM* pEventsClient_ReceiveFSM, urn_jaus_jss_core_Transport::Transport_ReceiveFSM* pTransport_ReceiveFSM)
4141: SlaveHandlerInterface(cmp, " CostMap2DClient" , 10.0 ),
42- logger (cmp->get_logger ().get_child(" CostMap2DClient" )),
43- p_tf_broadcaster(cmp)
42+ logger (cmp->get_logger ().get_child(" CostMap2DClient" ))
4443{
4544
4645 /*
@@ -50,6 +49,10 @@ CostMap2DClient_ReceiveFSM::CostMap2DClient_ReceiveFSM(std::shared_ptr<iop::Comp
5049 */
5150 context = new CostMap2DClient_ReceiveFSMContext (*this );
5251
52+ this ->p_tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(
53+ cmp->get_node_parameters_interface (),
54+ cmp->get_node_topics_interface ()
55+ );
5356 this ->pAccessControlClient_ReceiveFSM = pAccessControlClient_ReceiveFSM;
5457 this ->pEventsClient_ReceiveFSM = pEventsClient_ReceiveFSM;
5558 this ->pTransport_ReceiveFSM = pTransport_ReceiveFSM;
@@ -191,7 +194,7 @@ void CostMap2DClient_ReceiveFSM::handleReportCostMap2DAction(ReportCostMap2D msg
191194 tf_msg.child_frame_id = this ->p_tf_frame_costmap ;
192195 }
193196 if (! tf_msg.child_frame_id .empty () && !tf_msg.header .frame_id .empty ()) {
194- p_tf_broadcaster. sendTransform (tf_msg);
197+ p_tf_broadcaster-> sendTransform (tf_msg);
195198 RCLCPP_DEBUG (logger, " tf %s -> %s (%.2f, %.2f), stamp: %d.%d" , tf_msg.header .frame_id .c_str (), tf_msg.child_frame_id .c_str (), tf_msg.transform .translation .x , tf_msg.transform .translation .y , tf_msg.header .stamp .sec , tf_msg.header .stamp .nanosec );
196199 }
197200
0 commit comments