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

Jump to bottom

Ability to toggle automatic point collection during recording #56

Closed
wants to merge 5 commits into from
Closed

Ability to toggle automatic point collection during recording #56

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
609ab92
Toggle auto point collecting
jkaflik Aug 9, 2023
7f1a212
allow manual point collect only if auto collecting disabled
jkaflik Aug 9, 2023
cd279bd
OpenMowerApp update with auto point collecting
jkaflik Aug 9, 2023
9b1f69e
Dense polygon points for slicer
jkaflik Aug 13, 2023
2f43712
Merge branch 'main' into feature/toggle-auto-point-collecting
ClemensElflein Apr 19, 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
79 changes: 76 additions & 3 deletions src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -193,9 +193,11 @@ void AreaRecordingBehavior::enter() {
mow_area_sub = n->subscribe("/record_mowing", 100, &AreaRecordingBehavior::record_mowing_received, this);
nav_area_sub = n->subscribe("/record_navigation", 100, &AreaRecordingBehavior::record_navigation_received, this);

auto_point_collecting_sub = n->subscribe("/record_auto_point_collecting", 100, &AreaRecordingBehavior::record_auto_point_collecting, this);
collect_point_sub = n->subscribe("/record_collect_point", 100, &AreaRecordingBehavior::record_collect_point, this);

pose_sub = n->subscribe("/xbot_positioning/xb_pose", 100,
&AreaRecordingBehavior::pose_received, this);

}

void AreaRecordingBehavior::exit() {
Expand All @@ -212,6 +214,8 @@ void AreaRecordingBehavior::exit() {
polygon_sub.shutdown();
mow_area_sub.shutdown();
nav_area_sub.shutdown();
auto_point_collecting_sub.shutdown();
collect_point_sub.shutdown();
pose_sub.shutdown();
add_mowing_area_client.shutdown();
set_docking_point_client.shutdown();
Expand Down Expand Up @@ -269,7 +273,18 @@ void AreaRecordingBehavior::joy_received(const sensor_msgs::Joy &joy_msg) {
set_docking_position = true;
}


// use RB button for manual point collecting
// enable/disable auto point collecting with LB+RB
if (joy_msg.buttons[5] && !last_joy.buttons[5]) {
if (joy_msg.buttons[4] && !last_joy.buttons[4]) {
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this if will likely never happen as there is no separate state for joy buttons, so all four conditions will be not met (?)

Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yes, that would require pressing both buttons in the same loop

ROS_INFO_STREAM("LB+RB PRESSED, toggle auto point collecting");
auto_point_collecting = !auto_point_collecting;
ROS_INFO_STREAM("Auto point collecting: " << auto_point_collecting);
} else {
ROS_INFO_STREAM("RB PRESSED, collect point");
collect_point = true;
}
}

last_joy = joy_msg;
}
Expand Down Expand Up @@ -399,7 +414,11 @@ bool AreaRecordingBehavior::recordNewPolygon(geometry_msgs::Polygon &polygon, xb
tf2::Vector3 last_point(last.x, last.y, 0.0);
tf2::Vector3 current_point(pose_in_map.position.x, pose_in_map.position.y, 0.0);

if ((current_point - last_point).length() > 0.1) {
bool is_new_point_far_enough = (current_point - last_point).length() > NEW_POINT_MIN_DISTANCE;
bool is_point_auto_collected = auto_point_collecting && is_new_point_far_enough;
bool is_point_manual_collected = !auto_point_collecting && collect_point && is_new_point_far_enough;

if (is_point_auto_collected || is_point_manual_collected) {
geometry_msgs::Point32 pt;
pt.x = pose_in_map.position.x;
pt.y = pose_in_map.position.y;
Expand All @@ -421,6 +440,10 @@ bool AreaRecordingBehavior::recordNewPolygon(geometry_msgs::Polygon &polygon, xb

poly_viz.polygon.points.push_back(pt);
map_overlay_pub.publish(resultOverlay);

if (is_point_manual_collected) {
collect_point = false;
}
}
}

Expand Down Expand Up @@ -573,6 +596,15 @@ void AreaRecordingBehavior::handle_action(std::string action) {
} else if(action == "mower_logic:area_recording/record_dock") {
ROS_INFO_STREAM("Got record dock");
set_docking_position = true;
} else if(action == "mower_logic:area_recording/auto_point_collecting_enable") {
ROS_INFO_STREAM("Got enable auto point collecting");
auto_point_collecting = true;
} else if(action == "mower_logic:area_recording/auto_point_collecting_disable") {
ROS_INFO_STREAM("Got disable auto point collecting");
auto_point_collecting = false;
} else if(action == "mower_logic:area_recording/collect_point") {
ROS_INFO_STREAM("Got collect point");
collect_point = true;
}
}

Expand Down Expand Up @@ -612,6 +644,20 @@ AreaRecordingBehavior::AreaRecordingBehavior() {
record_dock_action.enabled = false;
record_dock_action.action_name = "Record Docking point";

xbot_msgs::ActionInfo auto_point_collecting_enable_action;
auto_point_collecting_enable_action.action_id = "auto_point_collecting_enable";
auto_point_collecting_enable_action.enabled = false;
auto_point_collecting_enable_action.action_name = "Enable automatic point collecting";

xbot_msgs::ActionInfo auto_point_collecting_disable_action;
auto_point_collecting_disable_action.action_id = "auto_point_collecting_disable";
auto_point_collecting_disable_action.enabled = false;
auto_point_collecting_disable_action.action_name = "Disable automatic point collecting";

xbot_msgs::ActionInfo collect_point_action;
collect_point_action.action_id = "collect_point";
collect_point_action.enabled = false;
collect_point_action.action_name = "Collect point";


actions.clear();
Expand All @@ -622,6 +668,9 @@ AreaRecordingBehavior::AreaRecordingBehavior() {
actions.push_back(exit_recording_mode_action);
actions.push_back(finish_discard_action);
actions.push_back(record_dock_action);
actions.push_back(auto_point_collecting_enable_action);
actions.push_back(auto_point_collecting_disable_action);
actions.push_back(collect_point_action);
}

void AreaRecordingBehavior::update_actions() {
Expand All @@ -639,6 +688,10 @@ void AreaRecordingBehavior::update_actions() {
actions[3].enabled = true;
actions[4].enabled = true;
actions[5].enabled = true;

// enable/disable auto point collecting
actions[7].enabled = !auto_point_collecting;
actions[8].enabled = auto_point_collecting;
} else {
// neither recording a polygon nor docking point. we can save if we have an outline and always discard
if(has_outline) {
Expand All @@ -655,6 +708,26 @@ void AreaRecordingBehavior::update_actions() {
}
}

actions[9].enabled = auto_point_collecting;

registerActions("mower_logic:area_recording", actions);
}
}

void AreaRecordingBehavior::record_auto_point_collecting(std_msgs::Bool state_msg) {
if (state_msg.data) {
ROS_INFO_STREAM("Recording auto point collecting enabled");
auto_point_collecting = true;
} else {
ROS_INFO_STREAM("Recording auto point collecting disabled");
auto_point_collecting = false;
}
}

void AreaRecordingBehavior::record_collect_point(std_msgs::Bool state_msg) {
if (state_msg.data) {
ROS_INFO_STREAM("Recording collect point");

collect_point = true;
}
}
12 changes: 11 additions & 1 deletion src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,8 @@
#include "std_msgs/Bool.h"
#include "xbot_msgs/MapOverlay.h"

#define NEW_POINT_MIN_DISTANCE 0.1

class AreaRecordingBehavior : public Behavior {
public:
static AreaRecordingBehavior INSTANCE;
Expand All @@ -66,7 +68,7 @@ class AreaRecordingBehavior : public Behavior {

ros::Subscriber joy_sub, pose_sub;

ros::Subscriber dock_sub, polygon_sub, mow_area_sub, nav_area_sub;
ros::Subscriber dock_sub, polygon_sub, mow_area_sub, nav_area_sub, auto_point_collecting_sub, collect_point_sub;

ros::ServiceClient add_mowing_area_client, set_docking_point_client;

Expand All @@ -83,6 +85,12 @@ class AreaRecordingBehavior : public Behavior {
bool set_docking_position = false;
bool has_outline = false;

// auto point collecting enabled to true points are collected automatically
// if distance is greater than NEW_POINT_MIN_DISTANCE during recording
// otherwise collect_point has to be set to true manually for each point to be recorded
bool auto_point_collecting = true;
bool collect_point = false;

visualization_msgs::MarkerArray markers;
visualization_msgs::Marker marker;

Expand All @@ -95,6 +103,8 @@ class AreaRecordingBehavior : public Behavior {
void record_polygon_received(std_msgs::Bool state_msg);
void record_mowing_received(std_msgs::Bool state_msg);
void record_navigation_received(std_msgs::Bool state_msg);
void record_auto_point_collecting(std_msgs::Bool state_msg);
void record_collect_point(std_msgs::Bool state_msg);

void update_actions();

Expand Down
Loading