diff --git a/wall_tracking_executor/include/wall_tracking_executor/wall_tracking_executor.hpp b/wall_tracking_executor/include/wall_tracking_executor/wall_tracking_executor.hpp index 91086f8..b4145cb 100644 --- a/wall_tracking_executor/include/wall_tracking_executor/wall_tracking_executor.hpp +++ b/wall_tracking_executor/include/wall_tracking_executor/wall_tracking_executor.hpp @@ -19,7 +19,10 @@ #include #include #include - +#include +#include +#include +#include using WallTrackingAction = wall_tracking_msgs::action::WallTracking; using GoalHandleWallTracking = rclcpp_action::ServerGoalHandle; @@ -51,6 +54,7 @@ class WallTracking : public rclcpp::Node { void pub_open_place_detection(std::string open_place_detection); void gnss_pose_with_covariance_callback(geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg); void goal_pose_callback(geometry_msgs::msg::PoseStamped::ConstSharedPtr msg); + void alpha_callback(std_msgs::msg::Float32::ConstSharedPtr msg); // For wall_tracking action server rclcpp_action::GoalResponse handle_goal( @@ -75,6 +79,8 @@ class WallTracking : public rclcpp::Node { void feedbackCallback([[maybe_unused]] typename std::shared_ptr, [[maybe_unused]] const std::shared_ptr feedback); void resultCallback(const GoalHandleNavigateToPose::WrappedResult & result); + void addBehaviorStamedArray(std::string behavior_name); + void behaviorStampedPub(void); private: rclcpp::Subscription::SharedPtr scan_sub_; @@ -84,6 +90,7 @@ class WallTracking : public rclcpp::Node { rclcpp::Publisher::SharedPtr open_place_detection_pub_; rclcpp::Subscription::SharedPtr gnss_pose_with_covariance_sub_; rclcpp::Subscription::SharedPtr goal_pose_sub_; + rclcpp::Subscription::SharedPtr alpha_sub_; rclcpp_action::Server::SharedPtr wall_tracking_action_srv_; @@ -94,7 +101,10 @@ class WallTracking : public rclcpp::Node { rclcpp_action::Client::SendGoalOptions nav_send_goal_options_; rclcpp_action::Client::SharedPtr navigation_action_client_; - + + std::vector behavior_stamped_array_; + rclcpp::Publisher::SharedPtr behavior_stamped_array_pub_; + float distance_from_wall_; float distance_to_stop_; float max_linear_vel_; @@ -118,9 +128,11 @@ class WallTracking : public rclcpp::Node { bool open_place_linear_; std::vector select_angvel_; std::vector detection_div_deg_; - float pre_e_; + float pre_e_; bool gnss_nan_; bool recieved_nav_goal_; + bool only_navigate_open_place_flg_ = false; + float alpha_; }; } // namespace WallTracking diff --git a/wall_tracking_executor/src/wall_tracking_executor.cpp b/wall_tracking_executor/src/wall_tracking_executor.cpp index 8e2f63d..3f78aae 100644 --- a/wall_tracking_executor/src/wall_tracking_executor.cpp +++ b/wall_tracking_executor/src/wall_tracking_executor.cpp @@ -85,6 +85,13 @@ void WallTracking::init_sub() goal_pose_sub_ = this->create_subscription( "goal_pose", rclcpp::QoS(1), std::bind(&WallTracking::goal_pose_callback, this, std::placeholders::_1)); + alpha_sub_ = this->create_subscription( + "alpha", rclcpp::QoS(2), std::bind(&WallTracking::alpha_callback, this, std::placeholders::_1)); +} + +void WallTracking::alpha_callback(std_msgs::msg::Float32::ConstSharedPtr msg) +{ + alpha_ = msg->data; } void WallTracking::init_pub() @@ -92,6 +99,7 @@ void WallTracking::init_pub() cmd_vel_pub_ = this->create_publisher("cmd_vel", rclcpp::QoS(10)); open_place_arrived_pub_ = this->create_publisher("open_place_arrived", rclcpp::QoS(10)); open_place_detection_pub_ = this->create_publisher("open_place_detection", rclcpp::QoS(10)); + behavior_stamped_array_pub_ = this->create_publisher("behavior_stamped", rclcpp::QoS(10)); } void WallTracking::init_action() @@ -130,15 +138,20 @@ void WallTracking::feedbackCallback([[maybe_unused]] typename std::shared_ptrget_logger(), "Goal succeeded!"); recieved_nav_goal_ = false; - RCLCPP_INFO(this->get_logger(), "Recieved nav goal: %d", recieved_nav_goal_); + RCLCPP_INFO(this->get_logger(), "Goal succeeded!"); + addBehaviorStamedArray("Navigation Succeeded"); + behaviorStampedPub(); + // RCLCPP_INFO(this->get_logger(), "Recieved nav goal: %d", recieved_nav_goal_); break; case rclcpp_action::ResultCode::ABORTED: RCLCPP_WARN(this->get_logger(), "Goal was aborted"); + addBehaviorStamedArray("Navigation Aborted"); + behaviorStampedPub(); break; case rclcpp_action::ResultCode::CANCELED: RCLCPP_WARN(this->get_logger(), "Goal was canceled"); + addBehaviorStamedArray("Navigation canceled"); // recieved_nav_goal_ = true; break; default: @@ -147,6 +160,19 @@ void WallTracking::resultCallback(const GoalHandleNavigateToPose::WrappedResult } } +void WallTracking::behaviorStampedPub(void) +{ + rclcpp::Rate pub_rate(1); + for(auto &b: behavior_stamped_array_){ + wall_tracking_msgs::msg::BehaviorStamped tmp; + tmp.behavior_name = b.behavior_name; + tmp.stamp = b.stamp; + behavior_stamped_array_pub_->publish(tmp); + pub_rate.sleep(); + } + behavior_stamped_array_.clear(); +} + void WallTracking::init_variable() { ei_ = 0.0; @@ -191,11 +217,11 @@ void WallTracking::scan_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr msg // float p = scan_data_->openPlaceCheck(-90., 90., open_place_distance_, per, mean); scan_data_->openPlaceCheck(-90., 90., open_place_distance_, per, mean); open_place_ = !open_place_ ? (per >= 0.7) : per >= 0.4; - // if(gnss_nan_) open_place_ = false; + if(gnss_nan_) open_place_ = false; cmd_vel_ = !open_place_ ? max_linear_vel_ : vel_open_place_; } pub_open_place_arrived(open_place_); - if(wall_tracking_flg_) navigateOpenPlace(); + if(wall_tracking_flg_ && recieved_nav_goal_) navigateOpenPlace(); else pub_cmd_vel(0., 0.); // RCLCPP_INFO(this->get_logger(), "update scan data"); } @@ -203,6 +229,8 @@ void WallTracking::scan_callback(sensor_msgs::msg::LaserScan::ConstSharedPtr msg void WallTracking::goal_pose_callback(geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) { nav_goal_msgs_.pose = *msg; + behavior_stamped_array_.clear(); + addBehaviorStamedArray("Navigation Start"); recieved_nav_goal_ = true; RCLCPP_INFO(this->get_logger(), "Recieved nav goal: %d", recieved_nav_goal_); navigation_action_client_->async_send_goal(nav_goal_msgs_, nav_send_goal_options_); @@ -296,6 +324,15 @@ void WallTracking::navigateOpenPlace() } } +void WallTracking::addBehaviorStamedArray(std::string behavior_name) +{ + wall_tracking_msgs::msg::BehaviorStamped tmp_behavior_stamped; + tmp_behavior_stamped.behavior_name = behavior_name; + tmp_behavior_stamped.stamp = now(); + behavior_stamped_array_.push_back(tmp_behavior_stamped); + // RCLCPP_INFO(this->get_logger(), "Num of behavior stamped array: %ld", behavior_stamped_array_.size()); +} + void WallTracking::pub_open_place_arrived(bool open_place_arrived) { open_place_arrived_msg_.data = open_place_arrived; @@ -340,6 +377,7 @@ void WallTracking::execute( RCLCPP_INFO(this->get_logger(), "Send cancel navigation to server"); } rclcpp::sleep_for(1000ms); + addBehaviorStamedArray("WallTracking Start"); RCLCPP_INFO(this->get_logger(), "EXECUTE"); const auto goal = goal_handle->get_goal(); std::shared_ptr feedback = std::make_shared(); @@ -348,13 +386,16 @@ void WallTracking::execute( rclcpp::Rate loop_rate(20); while (rclcpp::ok()) { if (goal_handle->is_canceling()) { + wall_tracking_flg_ = false; result->get = false; goal_handle->canceled(result); pub_cmd_vel(0.0, 0.0); + addBehaviorStamedArray("WallTracking Cancel"); RCLCPP_INFO(this->get_logger(), "Goal Canceled"); if(recieved_nav_goal_){ navigation_action_client_->async_send_goal(nav_goal_msgs_, nav_send_goal_options_); + addBehaviorStamedArray("Navigation Resume"); RCLCPP_INFO(this->get_logger(), "Resume navigation"); } return; diff --git a/wall_tracking_msgs/CMakeLists.txt b/wall_tracking_msgs/CMakeLists.txt index a10ee81..10217cc 100644 --- a/wall_tracking_msgs/CMakeLists.txt +++ b/wall_tracking_msgs/CMakeLists.txt @@ -20,15 +20,22 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() + +set(msg_files + "msg/BehaviorStamped.msg" + "msg/BehaviorStampedArray.msg" +) + set(action_files "action/WallTracking.action" ) rosidl_generate_interfaces(${PROJECT_NAME} ${action_files} + ${msg_files} DEPENDENCIES builtin_interfaces ADD_LINTER_TESTS ) -ament_auto_package() \ No newline at end of file +ament_auto_package() diff --git a/wall_tracking_msgs/msg/BehaviorStamped.msg b/wall_tracking_msgs/msg/BehaviorStamped.msg new file mode 100644 index 0000000..a9912dd --- /dev/null +++ b/wall_tracking_msgs/msg/BehaviorStamped.msg @@ -0,0 +1,2 @@ +string behavior_name +builtin_interfaces/Time stamp diff --git a/wall_tracking_msgs/msg/BehaviorStampedArray.msg b/wall_tracking_msgs/msg/BehaviorStampedArray.msg new file mode 100644 index 0000000..2896660 --- /dev/null +++ b/wall_tracking_msgs/msg/BehaviorStampedArray.msg @@ -0,0 +1 @@ +wall_tracking_msgs/BehaviorStamped[] behavior_stamped_array