Merge branch 'main' into perimeter

src/mower_comms/src/ll_datatypes.h

@@ -23,10 +23,11 @@
 #define PACKET_ID_LL_STATUS 1
 #define PACKET_ID_LL_IMU 2
 #define PACKET_ID_LL_UI_EVENT 3
+#define PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ 0x21 // ll_high_level_config and request config from receiver
+#define PACKET_ID_LL_HIGH_LEVEL_CONFIG_RSP 0x22 // ll_high_level_config response
 #define PACKET_ID_LL_HEARTBEAT 0x42
 #define PACKET_ID_LL_HIGH_LEVEL_STATE 0x43
 
-
 #pragma pack(push, 1)
 struct ll_status {
     // Type of this message. Has to be PACKET_ID_LL_STATUS.
@@ -109,4 +110,23 @@ struct ll_high_level_state {
 } __attribute__((packed));
 #pragma pack(pop)
 
+#define LL_HIGH_LEVEL_CONFIG_MAX_COMMS_VERSION 1           // Max. comms packet version supported by this open_mower_ros
+#define LL_HIGH_LEVEL_CONFIG_BIT_DFPIS5V 1 << 0            // Enable full sound via mower_config env var "OM_DFP_IS_5V"
+#define LL_HIGH_LEVEL_CONFIG_BIT_EMERGENCY_INVERSE 1 << 1  // Sample, for possible future usage, i.e. for SA-Type emergency
+
+typedef char iso639_1[2];  // Two char ISO 639-1 language code
+
+#pragma pack(push, 1)
+struct ll_high_level_config {
+    uint8_t type = PACKET_ID_LL_HIGH_LEVEL_CONFIG_RSP;               // By default, respond only (for now)
+    uint8_t comms_version = LL_HIGH_LEVEL_CONFIG_MAX_COMMS_VERSION;  // Increasing comms packet-version number for packet compatibility (n > 0)
+    uint8_t config_bitmask = 0;                                      // See LL_HIGH_LEVEL_CONFIG_BIT_*
+    int8_t volume;                                                   // Volume (0-100%) feedback (if directly changed via CoverUI). -1 = don't change
+    iso639_1 language;                                               // ISO 639-1 (2-char) language code (en, de, ...)
+    uint16_t spare1 = 0;                                             // Spare for future use
+    uint16_t spare2 = 0;                                             // Spare for future use
+    uint16_t crc;
+} __attribute__((packed));
+#pragma pack(pop)
+
 #endif

src/mower_comms/src/mower_comms.cpp

@@ -38,6 +38,8 @@
 #include <xbot_msgs/WheelTick.h>
 #include "mower_msgs/HighLevelStatus.h"
 
+#include <bitset>
+
 ros::Publisher status_pub;
 ros::Publisher wheel_tick_pub;
 
@@ -65,6 +67,10 @@ float speed_l = 0, speed_r = 0, speed_mow = 0;
 double wheel_ticks_per_m = 0.0;
 double wheel_distance_m = 0.0;
 
+bool dfp_is_5v = false;       // DFP is set to 5V Vcc
+std::string language = "en";  // ISO-639-1 (2 char) language code
+int volume = -1;              // -1 = don't change, 0-100 = volume (%)
+
 // Serial port and buffer for the low level connection
 serial::Serial serial_port;
 uint8_t out_buf[1000];
@@ -228,6 +234,37 @@ void publishStatus() {
     wheel_tick_pub.publish(wheel_tick_msg);
 }
 
+void publishLowLevelConfig() {
+    if (!serial_port.isOpen() || !allow_send)
+        return;
+
+    struct ll_high_level_config config_pkt;
+
+    config_pkt.volume = volume;
+    for (unsigned int i = 0; i < sizeof(config_pkt.language) / sizeof(char); i++) {
+        config_pkt.language[i] = language[i];
+    }
+    // Set config_bitmask flags
+    (dfp_is_5v) ? config_pkt.config_bitmask |= LL_HIGH_LEVEL_CONFIG_BIT_DFPIS5V : config_pkt.config_bitmask &= ~LL_HIGH_LEVEL_CONFIG_BIT_DFPIS5V;
+
+    crc.reset();
+    crc.process_bytes(&config_pkt, sizeof(struct ll_high_level_config) - 2);
+    config_pkt.crc = crc.checksum();
+
+    size_t encoded_size = cobs.encode((uint8_t *)&config_pkt, sizeof(struct ll_high_level_config), out_buf);
+    out_buf[encoded_size] = 0;
+    encoded_size++;
+
+    try {
+        ROS_INFO_STREAM("Send ll_high_level_config packet 0x" << std::hex << +config_pkt.type << " with comms_version=" << +config_pkt.comms_version
+                                                              << ", config_bitmask=0b" << std::bitset<8>(config_pkt.config_bitmask)
+                                                              << ", volume=" << std::dec << +config_pkt.volume << ", language='" << config_pkt.language << "'");
+        serial_port.write(out_buf, encoded_size);
+    } catch (std::exception &e) {
+        ROS_ERROR_STREAM("Error writing to serial port");
+    }
+}
+
 void publishActuatorsTimerTask(const ros::TimerEvent &timer_event) {
     publishActuators();
     publishStatus();
@@ -343,6 +380,24 @@ void handleLowLevelUIEvent(struct ll_ui_event *ui_event) {
 
 }
 
+void handleLowLevelConfig(struct ll_high_level_config *config_pkt) {
+    ROS_INFO_STREAM("Received ll_high_level_config packet 0x" << std::hex << +config_pkt->type
+                                                              << " with comms_version=" << +config_pkt->comms_version
+                                                              << ", config_bitmask=0b" << std::bitset<8>(config_pkt->config_bitmask)
+                                                              << ", volume=" << std::dec << +config_pkt->volume << ", language='" << config_pkt->language << "'");
+
+    // TODO: Handle announced comms_version once required
+
+    // We're not interested in the received langauge setting (yet)
+
+    // We're not interested in the received volume setting (yet)
+
+    if (config_pkt->type == PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ ||                      // Config requested
+        config_pkt->config_bitmask & LL_HIGH_LEVEL_CONFIG_BIT_DFPIS5V != dfp_is_5v) {  // Our DFP_IS_5V setting is leading
+        publishLowLevelConfig();
+    }
+}
+
 void handleLowLevelStatus(struct ll_status *status) {
     std::unique_lock<std::mutex> lk(ll_status_mutex);
     last_ll_status = *status;
@@ -416,6 +471,10 @@ int main(int argc, char **argv) {
 
     speed_l = speed_r = speed_mow = 0;
 
+    paramNh.getParam("dfp_is_5v", dfp_is_5v);
+    paramNh.getParam("language", language);
+    paramNh.getParam("volume", volume);
+    ROS_INFO_STREAM("DFP is set to 5V [boolean]: " << dfp_is_5v << ", language: '" << language << "', volume: " << volume);
 
     // Setup XESC interfaces
     if(mowerParamNh.hasParam("xesc_type")) {
@@ -526,6 +585,15 @@ int main(int argc, char **argv) {
                                             "Low Level Board sent a valid packet with the wrong size. Type was UI_EVENT");
                                 }
                                 break;
+                            case PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ:
+                            case PACKET_ID_LL_HIGH_LEVEL_CONFIG_RSP:
+                                if (data_size == sizeof(struct ll_high_level_config)) {
+                                    handleLowLevelConfig((struct ll_high_level_config *)buffer_decoded);
+                                } else {
+                                    ROS_INFO_STREAM(
+                                        "Low Level Board sent a valid packet with the wrong size. Type was CONFIG_* (0x" << std::hex << buffer_decoded[0] << ")");
+                                }
+                                break;
                             default:
                                 ROS_INFO_STREAM("Got unknown packet from Low Level Board");
                                 break;

src/mower_logic/cfg/MowerLogic.cfg

@@ -10,7 +10,8 @@ gen.add("docking_distance", double_t, 0, "Distance to drive forward during docki
 gen.add("docking_approach_distance", double_t, 0, "Distance to approach docking point", 1.5, 0, 5)
 gen.add("docking_retry_count", int_t, 0, "How often should we retry docking", 4, 1, 10)
 gen.add("perimeter_signal",int_t, 0, "Which perimeter signal should be used? 0-None, positive number gives signal number and uses counterclockwise docking, negative numbers use clockwise docking", 0, -2, 2)
-gen.add("outline_count", int_t, 0, "Outline count to mow before filling", 3, 0, 10)
+gen.add("outline_count", int_t, 0, "Outline count to mow before filling", 3, 0, 255)
+gen.add("outline_overlap_count", int_t, 0, "Outlines to overlap with fill", 0, 0, 255)
 gen.add("outline_offset", double_t, 0, "Additional outer outline offset, use positive values for safety, negative values to enlarge the area", 0.0, -1.0, 1.0)
 gen.add("mow_angle_offset", double_t, 0, "Mowing angle offset", 0, -180, 180)
 gen.add("mow_angle_offset_is_absolute", bool_t, 0, "If true then the offset is relative to East, if false then the offset is relative to the auto-detected angle.", False)

src/mower_logic/src/mower_logic/behaviors/MowingBehavior.cpp

@@ -171,6 +171,7 @@ bool MowingBehavior::create_mowing_plan(int area_index) {
     slic3r_coverage_planner::PlanPath pathSrv;
     pathSrv.request.angle = angle;
     pathSrv.request.outline_count = config.outline_count;
+    pathSrv.request.outline_overlap_count = config.outline_overlap_count;
     pathSrv.request.outline = mapSrv.response.area.area;
     pathSrv.request.holes = mapSrv.response.area.obstacles;
     pathSrv.request.fill_type = slic3r_coverage_planner::PlanPathRequest::FILL_LINEAR;

src/mower_map/CMakeLists.txt

@@ -20,6 +20,7 @@ find_package(catkin REQUIRED
         grid_map_filters
         grid_map_cv
         rosbag
+        xbot_msgs
         )
 
 ## System dependencies are found with CMake's conventions

src/open_mower/config/mower_config.sh.example

@@ -55,6 +55,25 @@ export OM_MOWER_GAMEPAD="xbox360"
 # Output will be stored in your $HOME
 # export OM_ENABLE_RECORDING_ALL=False
 
+# Full Sound Support - But read on carefully:
+# Up to (and inclusive) OM hardware version 0.13.x, DFPlayer's power supply is set by default to 3.3V.
+# This is done by solder jumper "JP1" whose board track is by default at 3.3V.
+# If you manually opened the 3.3V track and solder a bridge to 5V, then you can indicate it here to get full sound support.
+# DO NOT enable "OM_DFP_IS_5V=True" if you haven't changed it in real. You might risk your "Raspberry Pico"!
+# And even if the Pico isn't expensive, it's a torture to replace it.
+# Full details are available here: https://github.com/ClemensElflein/OpenMower/blob/main/Firmware/LowLevel/README-Sound%2C%20DFPIS5V.md
+# export OM_DFP_IS_5V=False
+#
+# Language as ISO-639-1 code string (currently only used by sound).
+# Supported languages: en|de
+# export OM_LANGUAGE="en"
+#
+# Sound volume (%)
+# Supported values:
+# 0-100 = Set sound volume on OM start, ignoring a previously set volume level (i.e. changed by CoverUI)
+#    -1 = Don't change a previously set volume level (i.e. changed by CoverUI)
+# export OM_VOLUME=-1
+
 ################################
 ##        GPS Settings        ##
 ################################
@@ -105,6 +124,9 @@ export OM_UNDOCK_DISTANCE=2.0
 # How many outlines should the mover drive. It's not recommended to set this below 4.
 export OM_OUTLINE_COUNT=4
 
+# How many outlines should the fill (lanes) overlap
+export OM_OUTLINE_OVERLAP_COUNT=0
+
 # Mowing angle offset -180 deg - +180 deg, 0 = east, -90 = north. If mowing angle offset is not absolute it gets added to the auto detected angle which is set by the first 2 m of recorded outline.
 export OM_MOWING_ANGLE_OFFSET=0
 export OM_MOWING_ANGLE_OFFSET_IS_ABSOLUTE=False

src/open_mower/launch/include/_comms.launch

@@ -11,6 +11,9 @@
         <rosparam if="$(eval env('OM_MOWER')=='CUSTOM')" file="$(env HOME)/mower_params/comms_params.yaml"/>
         <param name="wheel_ticks_per_m" value="$(env OM_WHEEL_TICKS_PER_M)"/>
         <param name="wheel_distance_m" value="$(env OM_WHEEL_DISTANCE_M)"/>
+        <param name="dfp_is_5v" value="$(optenv OM_DFP_IS_5V False)"/>
+        <param name="language" value="$(optenv OM_LANGUAGE en)"/>
+        <param name="volume" value="$(optenv OM_VOLUME -1)"/>
     </node>
 
     <include file="$(find open_mower)/launch/include/_gps.launch" unless="$(optenv OM_NO_GPS False)">

src/open_mower/launch/open_mower.launch

@@ -17,6 +17,7 @@
         <param name="enable_mower" value="$(env OM_ENABLE_MOWER)"/>
         <param name="battery_empty_voltage" value="$(env OM_BATTERY_EMPTY_VOLTAGE)"/>
         <param name="outline_count" value="$(env OM_OUTLINE_COUNT)"/>
+        <param name="outline_overlap_count" value="$(optenv OM_OUTLINE_OVERLAP_COUNT 0)"/>
         <param name="outline_offset" value="$(env OM_OUTLINE_OFFSET)"/>
         <param name="mow_angle_offset" value="$(optenv OM_MOWING_ANGLE_OFFSET 0)"/>
         <param name="mow_angle_offset_is_absolute" value="$(optenv OM_MOWING_ANGLE_OFFSET_IS_ABSOLUTE False)"/>

src/open_mower/launch/sim_mower_logic.launch

@@ -11,9 +11,22 @@
     <node pkg="slic3r_coverage_planner" type="slic3r_coverage_planner" name="slic3r_coverage_planner" output="screen" required="true" />
     <node pkg="mower_logic" type="mower_logic" name="mower_logic" output="log" required="true">
         <param name="ignore_gps_errors" value="true"/>
-        <param name="outline_count" value="5"/>
         <param name="gps_wait_time" value="0"/>
-        <param name="undock_distance" value="0.1"/>
+        <param name="automatic_mode" value="$(optenv OM_AUTOMATIC_MODE 0)"/>
+        <param name="docking_distance" value="$(env OM_DOCKING_DISTANCE)"/>
+        <param name="undock_distance" value="$(env OM_UNDOCK_DISTANCE)"/>
+        <param name="tool_width" value="$(env OM_TOOL_WIDTH)"/>
+        <param name="enable_mower" value="$(env OM_ENABLE_MOWER)"/>
+        <param name="battery_empty_voltage" value="$(env OM_BATTERY_EMPTY_VOLTAGE)"/>
+        <param name="outline_count" value="$(env OM_OUTLINE_COUNT)"/>
+        <param name="outline_overlap_count" value="$(optenv OM_OUTLINE_OVERLAP_COUNT 0)"/>
+        <param name="outline_offset" value="$(env OM_OUTLINE_OFFSET)"/>
+        <param name="mow_angle_offset" value="$(optenv OM_MOWING_ANGLE_OFFSET 0)"/>
+        <param name="mow_angle_offset_is_absolute" value="$(optenv OM_MOWING_ANGLE_OFFSET_IS_ABSOLUTE False)"/>
+        <param name="mow_angle_increment" value="$(optenv OM_MOWING_ANGLE_INCREMENT 0)"/>
+        <param name="battery_full_voltage" value="$(env OM_BATTERY_FULL_VOLTAGE)"/>
+        <param name="motor_hot_temperature" value="$(env OM_MOWING_MOTOR_TEMP_HIGH)"/>
+        <param name="motor_cold_temperature" value="$(env OM_MOWING_MOTOR_TEMP_LOW)"/>
     </node>
 
     <node pkg="twist_mux" type="twist_mux" name="twist_mux" output="screen">
@@ -42,4 +55,4 @@
         <param name="sensor_ids" value="$(optenv OM_HEATMAP_SENSOR_IDS)" />
     </node>
 
-</launch>
+</launch>