From c51a20448bd0b026d0e3102c8fb0010ea97cbdf4 Mon Sep 17 00:00:00 2001 From: Ollie Walsh Date: Mon, 14 Oct 2024 01:40:32 +0100 Subject: [PATCH 1/2] Add manual mower control actions in AreaRecording mode Previously users would call mower_comms setMowEnabled directly to enable the mower while in AreaRecording mode. However this should not have been possible since AreaRecording::mower_enabled() always returned false. mower_logic now confirms that the mower state is as expected, so bypassing it in this way no longer works. Add actions to toggle AreaRecording::mower_enabled() instead. --- .../behaviors/AreaRecordingBehavior.cpp | 26 +++++++++++++++++-- .../behaviors/AreaRecordingBehavior.h | 2 ++ 2 files changed, 26 insertions(+), 2 deletions(-) diff --git a/src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.cpp b/src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.cpp index 61cdac8c..7dc46509 100644 --- a/src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.cpp +++ b/src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.cpp @@ -152,6 +152,7 @@ void AreaRecordingBehavior::enter() { has_outline = false; is_mowing_area = false; is_navigation_area = false; + manual_mowing = false; update_actions(); @@ -217,8 +218,7 @@ bool AreaRecordingBehavior::needs_gps() { } bool AreaRecordingBehavior::mower_enabled() { - // No mower during docking - return false; + return manual_mowing; } void AreaRecordingBehavior::pose_received(const xbot_msgs::AbsolutePose::ConstPtr &msg) { @@ -578,6 +578,12 @@ void AreaRecordingBehavior::handle_action(std::string action) { } else if (action == "mower_logic:area_recording/collect_point") { ROS_INFO_STREAM("Got collect point"); collect_point = true; + } else if (action == "mower_logic:area_recording/start_manual_mowing") { + ROS_INFO_STREAM("Starting manual mowing"); + manual_mowing = true; + } else if (action == "mower_logic:area_recording/stop_manual_mowing") { + ROS_INFO_STREAM("Stopping manual mowing"); + manual_mowing = false; } update_actions(); } @@ -633,6 +639,16 @@ AreaRecordingBehavior::AreaRecordingBehavior() { collect_point_action.enabled = false; collect_point_action.action_name = "Collect point"; + xbot_msgs::ActionInfo start_manual_mowing_action; + start_manual_mowing_action.action_id = "start_manual_mowing"; + start_manual_mowing_action.enabled = false; + start_manual_mowing_action.action_name = "Start manual mowing"; + + xbot_msgs::ActionInfo stop_manual_mowing_action; + stop_manual_mowing_action.action_id = "stop_manual_mowing"; + stop_manual_mowing_action.enabled = false; + stop_manual_mowing_action.action_name = "Stop manual mowing"; + actions.clear(); actions.push_back(start_recording_action); actions.push_back(stop_recording_action); @@ -644,6 +660,8 @@ AreaRecordingBehavior::AreaRecordingBehavior() { actions.push_back(auto_point_collecting_enable_action); actions.push_back(auto_point_collecting_disable_action); actions.push_back(collect_point_action); + actions.push_back(start_manual_mowing_action); + actions.push_back(stop_manual_mowing_action); } void AreaRecordingBehavior::update_actions() { @@ -681,6 +699,10 @@ void AreaRecordingBehavior::update_actions() { actions[6].enabled = true; } } + // start_manual_mowing + actions[10].enabled = !manual_mowing; + // stop manual mowing + actions[11].enabled = manual_mowing; registerActions("mower_logic:area_recording", actions); } diff --git a/src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.h b/src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.h index 823e7bef..1f3b26b4 100644 --- a/src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.h +++ b/src/mower_logic/src/mower_logic/behaviors/AreaRecordingBehavior.h @@ -89,6 +89,8 @@ class AreaRecordingBehavior : public Behavior { bool auto_point_collecting = true; bool collect_point = false; + bool manual_mowing = false; + visualization_msgs::MarkerArray markers; visualization_msgs::Marker marker; From 2d3d8443cb27a7df8d351a5fc67f4ffe5c629110 Mon Sep 17 00:00:00 2001 From: Ollie Walsh Date: Sat, 18 Jan 2025 00:13:44 +0000 Subject: [PATCH 2/2] Avoid mower_comms cmd_vel timeout when joystick is enabled If no twist message is received from the joystick after 10s then send a zero cmd_vel. --- src/mower_logic/src/mower_logic/mower_logic.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/mower_logic/src/mower_logic/mower_logic.cpp b/src/mower_logic/src/mower_logic/mower_logic.cpp index c3bf169d..22ec51f1 100644 --- a/src/mower_logic/src/mower_logic/mower_logic.cpp +++ b/src/mower_logic/src/mower_logic/mower_logic.cpp @@ -75,6 +75,7 @@ ros::Time pose_time(0.0); xbot_msgs::AbsolutePose last_pose; ros::Time status_time(0.0); mower_msgs::Status last_status; +ros::Time joy_vel_time(0.0); ros::Time last_good_gps(0.0); @@ -494,6 +495,12 @@ void checkSafety(const ros::TimerEvent &timer_event) { } } + if (currentBehavior != nullptr && currentBehavior->redirect_joystick()) { + if (ros::Time::now() - joy_vel_time > ros::Duration(10)) { + stopMoving(); // To avoid cmd_vel receive timeout in mower_comms + } + } + // enable the mower (if not aleady) if mowerAllowed is still true after checks and bahavior agrees setMowerEnabled(currentBehavior != nullptr && mowerAllowed && currentBehavior->mower_enabled()); @@ -636,6 +643,7 @@ void actionReceived(const std_msgs::String::ConstPtr &action) { } void joyVelReceived(const geometry_msgs::Twist::ConstPtr &joy_vel) { + joy_vel_time = ros::Time::now(); if (currentBehavior && currentBehavior->redirect_joystick()) { cmd_vel_pub.publish(joy_vel); }