-
Notifications
You must be signed in to change notification settings - Fork 1.2k
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.
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
[Collision Monitor] Add a watchdog mechanism #3880
Changes from all commits
71d6044
937bbe5
787bf32
d52ea3a
fb2b676
cc050c1
e611f1d
f37280b
64f7cc0
94587d0
e54293f
73e6cbb
73609f7
fcc1186
030ec7d
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -378,10 +378,25 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) | |
// Points array collected from different data sources in a robot base frame | ||
std::vector<Point> collision_points; | ||
|
||
// By default - there is no action | ||
Action robot_action{DO_NOTHING, cmd_vel_in, ""}; | ||
// Polygon causing robot action (if any) | ||
std::shared_ptr<Polygon> action_polygon; | ||
|
||
// Fill collision_points array from different data sources | ||
for (std::shared_ptr<Source> source : sources_) { | ||
if (source->getEnabled()) { | ||
source->getData(curr_time, collision_points); | ||
if (!source->getData(curr_time, collision_points) && | ||
source->getSourceTimeout().seconds() != 0.0) | ||
{ | ||
action_polygon = nullptr; | ||
robot_action.polygon_name = "invalid source"; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Please change this to There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Same as above. |
||
robot_action.action_type = STOP; | ||
robot_action.req_vel.x = 0.0; | ||
robot_action.req_vel.y = 0.0; | ||
robot_action.req_vel.tw = 0.0; | ||
break; | ||
} | ||
} | ||
} | ||
|
||
|
@@ -412,11 +427,6 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) | |
collision_points_marker_pub_->publish(std::move(marker_array)); | ||
} | ||
|
||
// By default - there is no action | ||
Action robot_action{DO_NOTHING, cmd_vel_in, ""}; | ||
// Polygon causing robot action (if any) | ||
std::shared_ptr<Polygon> action_polygon; | ||
|
||
for (std::shared_ptr<Polygon> polygon : polygons_) { | ||
if (!polygon->getEnabled()) { | ||
continue; | ||
|
@@ -545,10 +555,18 @@ void CollisionMonitor::notifyActionState( | |
const Action & robot_action, const std::shared_ptr<Polygon> action_polygon) const | ||
{ | ||
if (robot_action.action_type == STOP) { | ||
RCLCPP_INFO( | ||
get_logger(), | ||
"Robot to stop due to %s polygon", | ||
action_polygon->getName().c_str()); | ||
if (robot_action.polygon_name == "invalid source") { | ||
RCLCPP_WARN( | ||
get_logger(), | ||
"Robot to stop due to invalid source." | ||
" Either due to data not published yet, or to lack of new data received within the" | ||
" sensor timeout, or if impossible to transform data to base frame"); | ||
} else { | ||
RCLCPP_INFO( | ||
get_logger(), | ||
"Robot to stop due to %s polygon", | ||
action_polygon->getName().c_str()); | ||
} | ||
} else if (robot_action.action_type == SLOWDOWN) { | ||
RCLCPP_INFO( | ||
get_logger(), | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
We need to think, how to report the robot needs to stop, when the sources were outdated for Collision Detector. In the initial message, there is a detected polygons string array, but there is not ability what to do. So, I am thinking about publishing
CollisionDetectorState.msg
with emptypolygons[]
array, that will give the developer an information that something (not polygon) is causing robot to report the "collision". Or add one more bool variable into this message, as an alternative for it.There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
See last commit
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Now, the
polygons[]
is empty when robot detected to "collide" (stop) due to incorrect source. However, in normal operation, it will be also empty. Sorry, as I've missed this initially. Thus, we still seem to need to add invalid source feedback toCollisionDetectorState.msg
(andCollisionMonitorState.msg
).