Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feat/interlock 241014 #58

Merged
merged 3 commits into from
Oct 14, 2024
Merged

Feat/interlock 241014 #58

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
5ac0d9d
Update
makotoyoshigoe Sep 28, 2024
f026a84
Forget to committing
makotoyoshigoe Oct 14, 2024
6f21a5f
Add interlock
makotoyoshigoe Oct 14, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 11 additions & 3 deletions wall_tracking_executor/include/wall_tracking_executor/wall_tracking_executor.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,10 @@
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <nav2_msgs/action/navigate_to_pose.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>

#include <std_msgs/msg/float32.hpp>
#include <wall_tracking_msgs/msg/behavior_stamped_array.hpp>
#include <wall_tracking_msgs/msg/behavior_stamped.hpp>
#include <geometry_msgs/msg/pose_array.hpp>

using WallTrackingAction = wall_tracking_msgs::action::WallTracking;
using GoalHandleWallTracking = rclcpp_action::ServerGoalHandle<WallTrackingAction>;
Expand Down Expand Up @@ -75,6 +78,8 @@ class WallTracking : public rclcpp::Node {
void feedbackCallback([[maybe_unused]] typename std::shared_ptr<GoalHandleNavigateToPose>,
[[maybe_unused]] const std::shared_ptr<const typename NavigateToPose::Feedback> feedback);
void resultCallback(const GoalHandleNavigateToPose::WrappedResult & result);
void addBehaviorStamedArray(std::string behavior_name);
void behaviorStampedPub(void);

private:
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
Expand All @@ -94,7 +99,10 @@ class WallTracking : public rclcpp::Node {

rclcpp_action::Client<NavigateToPose>::SendGoalOptions nav_send_goal_options_;
rclcpp_action::Client<NavigateToPose>::SharedPtr navigation_action_client_;


std::vector<wall_tracking_msgs::msg::BehaviorStamped> behavior_stamped_array_;
rclcpp::Publisher<wall_tracking_msgs::msg::BehaviorStamped>::SharedPtr behavior_stamped_array_pub_;

float distance_from_wall_;
float distance_to_stop_;
float max_linear_vel_;
Expand All @@ -118,7 +126,7 @@ class WallTracking : public rclcpp::Node {
bool open_place_linear_;
std::vector<double> select_angvel_;
std::vector<double> detection_div_deg_;
float pre_e_;
float pre_e_;
bool gnss_nan_;
bool recieved_nav_goal_;
};
Expand Down
46 changes: 40 additions & 6 deletions wall_tracking_executor/src/wall_tracking_executor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,7 @@ void WallTracking::init_pub()
cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", rclcpp::QoS(10));
open_place_arrived_pub_ = this->create_publisher<std_msgs::msg::Bool>("open_place_arrived", rclcpp::QoS(10));
open_place_detection_pub_ = this->create_publisher<std_msgs::msg::String>("open_place_detection", rclcpp::QoS(10));
behavior_stamped_array_pub_ = this->create_publisher<wall_tracking_msgs::msg::BehaviorStamped>("behavior_stamped", rclcpp::QoS(10));
}

void WallTracking::init_action()
Expand Down Expand Up @@ -130,15 +131,20 @@ void WallTracking::feedbackCallback([[maybe_unused]] typename std::shared_ptr<Go
void WallTracking::resultCallback(const GoalHandleNavigateToPose::WrappedResult & result){
switch (result.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
RCLCPP_INFO(this->get_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:
Expand All @@ -147,6 +153,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;
Expand Down Expand Up @@ -191,21 +210,23 @@ 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");
}

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_);
if(!wall_tracking_flg_) navigation_action_client_->async_send_goal(nav_goal_msgs_, nav_send_goal_options_);
}

void WallTracking::gnss_callback(sensor_msgs::msg::NavSatFix::ConstSharedPtr msg)
Expand Down Expand Up @@ -235,7 +256,7 @@ void WallTracking::turn()
msg.linear.x = 0.0;
msg.angular.z = DEG2RAD(-45);
cmd_vel_pub_->publish(msg);
rclcpp::sleep_for(500ms);
rclcpp::sleep_for(100ms);
}

void WallTracking::wallTracking()
Expand Down Expand Up @@ -296,6 +317,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;
Expand Down Expand Up @@ -340,6 +370,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<WallTrackingAction::Feedback> feedback = std::make_shared<WallTrackingAction::Feedback>();
Expand All @@ -348,13 +379,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;
Expand Down
9 changes: 8 additions & 1 deletion wall_tracking_msgs/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
ament_auto_package()
2 changes: 2 additions & 0 deletions wall_tracking_msgs/msg/BehaviorStamped.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
string behavior_name
builtin_interfaces/Time stamp
1 change: 1 addition & 0 deletions wall_tracking_msgs/msg/BehaviorStampedArray.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
wall_tracking_msgs/BehaviorStamped[] behavior_stamped_array
Loading