3434#include " tf2_geometry_msgs/tf2_geometry_msgs.hpp"
3535#include " visualization_msgs/msg/marker_array.hpp"
3636#include " nav2_util/geometry_utils.hpp"
37- #include " nav2_costmap_2d/costmap_subscriber.hpp"
38- #include " nav2_costmap_2d/costmap_2d_ros.hpp"
39- #include " nav2_costmap_2d/costmap_topic_collision_checker.hpp"
4037#include " tf2_ros/transform_listener.h"
4138#include " tf2_ros/create_timer_ros.h"
39+ #include " tf2_ros/buffer.h"
4240
4341class QPushButton ;
4442
@@ -91,9 +89,7 @@ private Q_SLOTS:
9189 bool store_initial_pose_ = false ;
9290 bool initial_pose_stored_ = false ;
9391 bool loop_counter_stop_ = true ;
94-
9592 std::string loop_no_ = " 0"
96- geometry_msgs::msg::PoseWithCovarianceStamped initial_pose_;
9793
9894 // Call to send NavigateToPose action request for goal poses
9995 geometry_msgs::msg::PoseStamped convert_to_msg (
@@ -135,22 +131,8 @@ private Q_SLOTS:
135131 rclcpp::Subscription<nav2_msgs::action::NavigateThroughPoses::Impl::GoalStatusMessage>::SharedPtr
136132 nav_through_poses_goal_status_sub_;
137133
138- // map_pose subscriber for initial pose
139- rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
140- map_pose_sub_;
141-
142- // global_costmap and footprint subscriber
143- std::unique_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
144- std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
145-
146- // Collision checker
147- std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
148-
149- // Parameter client for getting parameters from costmap
150- std::shared_ptr<rclcpp::SyncParametersClient> param_client_;
151-
152- // Tf's for collision checker
153- std::shared_ptr<tf2_ros::Buffer> tf_;
134+ // Tf's for initial pose
135+ std::shared_ptr<tf2_ros::Buffer> tf2_buffer;
154136 std::shared_ptr<tf2_ros::TransformListener> transform_listener_;
155137
156138 // Goal-related state
@@ -195,6 +177,9 @@ private Q_SLOTS:
195177 QState * paused_wp_{nullptr };
196178 QState * resumed_wp_{nullptr };
197179
180+ QImage * image_{nullptr };
181+ QLabel * imgDisplayLabel{nullptr };
182+
198183 // The following states are added to allow for the state of the button to only expose reset
199184 // while the NavigateToPoses action is not active. While running, the user will be allowed to
200185 // cancel the action. The ROSActionTransition allows for the state of the action to be detected
@@ -210,7 +195,6 @@ private Q_SLOTS:
210195 std::vector<geometry_msgs::msg::PoseStamped> acummulated_poses_;
211196 std::vector<geometry_msgs::msg::PoseStamped> store_poses_;
212197
213- bool isWaypointValid (geometry_msgs::msg::Pose pose);
214198 // Publish the visual markers with the waypoints
215199 void updateWpNavigationMarkers ();
216200
0 commit comments