diff --git a/.gitignore b/.gitignore index edc2ef32..41801c87 100644 --- a/.gitignore +++ b/.gitignore @@ -59,3 +59,4 @@ CATKIN_IGNORE cmake-build-debug/ .vscode +.venv diff --git a/.gitmodules b/.gitmodules index 51549edc..3b04c33c 100644 --- a/.gitmodules +++ b/.gitmodules @@ -25,3 +25,6 @@ [submodule "src/lib/xbot_remote"] path = src/lib/xbot_remote url = https://github.com/ClemensElflein/xbot_remote +[submodule "src/lib/xbot_framework"] + path = src/lib/xbot_framework + url = https://github.com/ClemensElflein/xbot_framework.git diff --git a/docker/Dockerfile b/docker/Dockerfile index de55cda4..7490f43f 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -8,7 +8,7 @@ ENV DEBIAN_FRONTEND=noninteractive # - nginx (remove default site to free up port 80) RUN apt-get update && apt-get install --yes \ git \ - mosquitto \ + mosquitto python3.8-venv \ nginx && rm -rf /var/www /etc/nginx/sites-enabled/* \ rm -rf /var/lib/apt/lists/* @@ -43,7 +43,7 @@ COPY --link --from=fetch /opt/open_mower_ros/src/lib/slic3r_coverage_planner /op WORKDIR /opt/slic3r_coverage_planner_workspace RUN rosdep install --from-paths src --ignore-src --simulate | \ sed --expression '1d' | sort | tr -d '\n' | sed --expression 's/ apt-get install//g' > apt-install_list && \ - apt-get update && apt-get install --no-install-recommends --yes $(cat apt-install_list) && \ + apt-get update && apt-get install --no-install-recommends --yes $(cat apt-install_list) libasio-dev && \ rm -rf /var/lib/apt/lists/* apt-install_list RUN bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && catkin_make" RUN bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && source /opt/slic3r_coverage_planner_workspace/devel/setup.bash && catkin_make -DCMAKE_INSTALL_PREFIX=/opt/prebuilt/slic3r_coverage_planner install" @@ -77,7 +77,7 @@ COPY --link --from=slic3r /opt/prebuilt/slic3r_coverage_planner /opt/prebuilt/sl #Fetch the list of packages, this only changes if new dependencies have been added (only sometimes) COPY --link --from=dependencies /apt-install_list /apt-install_list RUN apt-get update && \ - apt-get install --no-install-recommends --yes $(cat /apt-install_list) && \ + apt-get install --no-install-recommends --yes $(cat /apt-install_list) libasio-dev && \ rm -rf /var/lib/apt/lists/* # This will already have the submodules initialized, no need to clone again diff --git a/services/diff_drive_service.json b/services/diff_drive_service.json new file mode 100644 index 00000000..1cc05fe9 --- /dev/null +++ b/services/diff_drive_service.json @@ -0,0 +1,65 @@ +{ + "inputs": [ + { + "id": 0, + "name": "Control Twist", + "type": "double[6]" + } + ], + "outputs": [ + { + "id": 0, + "name": "Actual Twist", + "type": "double[6]" + }, + { + "id": 1, + "name": "Left ESC Status", + "type": "uint8_t" + }, + { + "id": 2, + "name": "Left ESC Temperature", + "type": "float" + }, + { + "id": 3, + "name": "Left ESC Current", + "type": "float" + }, + { + "id": 4, + "name": "Right ESC Status", + "type": "uint8_t" + }, + { + "id": 5, + "name": "Right ESC Temperature", + "type": "float" + }, + { + "id": 6, + "name": "Right ESC Current", + "type": "float" + }, + { + "id": 7, + "name": "Wheel Ticks", + "type": "uint32_t[2]" + } + ], + "registers": [ + { + "id": 0, + "name": "Wheel Ticks Per Meter", + "type": "double" + }, + { + "id": 1, + "name": "Wheel Distance", + "type": "double" + } + ], + "type": "DiffDriveService", + "version": 1 +} diff --git a/services/docking_sensor_service.json b/services/docking_sensor_service.json new file mode 100644 index 00000000..f0d0353b --- /dev/null +++ b/services/docking_sensor_service.json @@ -0,0 +1,24 @@ +{ + "inputs": [], + "outputs": [ + { + "id": 0, + "name": "Detected Sensors Left", + "type": "uint8_t" + }, + { + "id": 1, + "name": "Detected Sensors Right", + "type": "uint8_t" + } + ], + "registers": [ + { + "id": 0, + "name": "Sensor Timeout Ms", + "type": "uint32_t" + } + ], + "type": "DockingSensorService", + "version": 1 +} diff --git a/services/emergency_service.json b/services/emergency_service.json new file mode 100644 index 00000000..38d68d01 --- /dev/null +++ b/services/emergency_service.json @@ -0,0 +1,29 @@ +{ + "inputs": [ + { + "id": 0, + "name": "Set Emergency", + "type": "uint8_t" + } + ], + "outputs": [ + { + "id": 0, + "name": "Emergency Active", + "type": "uint8_t" + }, + { + "id": 1, + "name": "Emergency Latch", + "type": "uint8_t" + }, + { + "id": 2, + "name": "Emergency Reason", + "type": "char[255]" + } + ], + "registers": [], + "type": "EmergencyService", + "version": 1 +} diff --git a/services/imu_service.json b/services/imu_service.json new file mode 100644 index 00000000..384378d8 --- /dev/null +++ b/services/imu_service.json @@ -0,0 +1,13 @@ +{ + "inputs": [], + "outputs": [ + { + "id": 0, + "name": "Axes", + "type": "double[9]" + } + ], + "registers": [], + "type": "ImuService", + "version": 1 +} diff --git a/services/lidar_service.json b/services/lidar_service.json new file mode 100644 index 00000000..542abd8a --- /dev/null +++ b/services/lidar_service.json @@ -0,0 +1,53 @@ +{ + "type": "LidarService", + "version": 1, + "inputs": [], + "outputs": [ + { + "id": 0, + "name": "AngleMinRad", + "type": "double" + }, + { + "id": 1, + "name": "AngleMaxRad", + "type": "double" + }, + { + "id": 2, + "name": "TimeIncrementSeconds", + "type": "float" + }, + { + "id": 3, + "name": "ScanTimeSeconds", + "type": "float" + }, + { + "id": 4, + "name": "RangeMinM", + "type": "float" + }, + { + "id": 5, + "name": "RangeMaxM", + "type": "float" + }, + { + "id": 6, + "name": "Ranges", + "type": "float[100]" + }, + { + "id": 7, + "name": "Intensities", + "type": "float[100]" + }, + { + "id": 8, + "name": "NewScan", + "type": "uint8_t" + } + ], + "registers": [] +} diff --git a/services/mower_service.json b/services/mower_service.json new file mode 100644 index 00000000..187b7354 --- /dev/null +++ b/services/mower_service.json @@ -0,0 +1,49 @@ +{ + "inputs": [ + { + "id": 0, + "name": "MowerEnabled", + "type": "uint8_t" + } + ], + "outputs": [ + { + "id": 0, + "name": "Mower Status", + "type": "uint8_t" + }, + { + "id": 1, + "name": "Rain Detected", + "type": "uint8_t" + }, + { + "id": 2, + "name": "Mower Running", + "type": "uint8_t" + }, + { + "id": 3, + "name": "Mower ESC Temperature", + "type": "float" + }, + { + "id": 4, + "name": "Mower Motor Temperature", + "type": "float" + }, + { + "id": 5, + "name": "Mower Motor Current", + "type": "float" + }, + { + "id": 6, + "name": "Mower Motor RPM", + "type": "float" + } + ], + "registers": [], + "type": "MowerService", + "version": 1 +} diff --git a/services/mower_ui_service.json b/services/mower_ui_service.json new file mode 100644 index 00000000..745b7bfb --- /dev/null +++ b/services/mower_ui_service.json @@ -0,0 +1,60 @@ +{ + "inputs": [ + { + "id": 0, + "name": "State ID", + "type": "uint8_t" + }, + { + "id": 1, + "name": "State Name", + "type": "char[100]" + }, + { + "id": 2, + "name": "Gps Quality", + "type": "float" + }, + { + "id": 3, + "name": "Battery Percent", + "type": "float" + } + ], + "outputs": [ + { + "id": 8, + "name": "Left ESC Temperature", + "type": "uint8_t" + }, + { + "id": 9, + "name": "Right ESC Temperature", + "type": "uint8_t" + }, + { + "id": 10, + "name": "Mow ESC Temperature", + "type": "uint8_t" + }, + { + "id": 11, + "name": "Mow Motor Temperature", + "type": "uint8_t" + } + ], + "registers": [ + { + "id": 0, + "name": "Wheel Ticks Per Meter", + "type": "double" + }, + { + "id": 1, + "name": "Wheel Distance", + "type": "double" + } + ], + "type": "MowerUiService", + "version": 1 +} diff --git a/services/position_service.json b/services/position_service.json new file mode 100644 index 00000000..abbbfca5 --- /dev/null +++ b/services/position_service.json @@ -0,0 +1,65 @@ +{ + "inputs": [ + { + "id": 0, + "name": "RTCM", + "type": "uint8_t[255]" + } + ], + "outputs": [ + { + "id": 0, + "name": "PositionXYZ", + "type": "double[3]" + }, + { + "id": 1, + "name": "PositionAccuracy", + "type": "double" + }, + { + "id": 2, + "name": "FixType", + "type": "char[10]" + }, + { + "id": 3, + "name": "MotionVectorXYZ", + "type": "double[3]" + }, + { + "id": 4, + "name": "MotionHeadingAndAccuracy", + "type": "double[2]" + }, + { + "id": 5, + "name": "VehicleHeadingAndAccuracy", + "type": "double[3]" + }, + { + "id": 6, + "name": "NMEA", + "type": "char[128]" + } + ], + "registers": [ + { + "id": 0, + "name": "Baudrate", + "type": "uint32_t" + }, + { + "id": 1, + "name": "Protocol", + "type": "char[10]" + }, + { + "id": 2, + "name": "DatumLatLonHeight", + "type": "double[3]" + } + ], + "type": "PositionService", + "version": 1 +} diff --git a/services/power_service.json b/services/power_service.json new file mode 100644 index 00000000..6d570c5c --- /dev/null +++ b/services/power_service.json @@ -0,0 +1,39 @@ +{ + "inputs": [ + { + "id": 0, + "name": "Charging Allowed", + "type": "uint8_t" + } + ], + "outputs": [ + { + "id": 0, + "name": "Charge Voltage", + "type": "float" + }, + { + "id": 1, + "name": "Charge Current", + "type": "float" + }, + { + "id": 2, + "name": "Battery Voltage", + "type": "float" + }, + { + "id": 3, + "name": "Charging Status", + "type": "char[25]" + }, + { + "id": 4, + "name": "Charger Enabled", + "type": "uint8_t" + } + ], + "registers": [], + "type": "PowerService", + "version": 1 +} diff --git a/src/lib/xbot_framework b/src/lib/xbot_framework new file mode 160000 index 00000000..b56fd70e --- /dev/null +++ b/src/lib/xbot_framework @@ -0,0 +1 @@ +Subproject commit b56fd70ef32b3baf72c12a87cf35eb5b0f053eec diff --git a/src/lib/xbot_positioning b/src/lib/xbot_positioning index a034f54e..7e80cd3a 160000 --- a/src/lib/xbot_positioning +++ b/src/lib/xbot_positioning @@ -1 +1 @@ -Subproject commit a034f54e1c33b3eec924d6de9b450b943ecb14f9 +Subproject commit 7e80cd3af894c64128fcd62d12d2de050f37301a diff --git a/src/mower_comms/CMakeLists.txt b/src/mower_comms_v1/CMakeLists.txt similarity index 92% rename from src/mower_comms/CMakeLists.txt rename to src/mower_comms_v1/CMakeLists.txt index d9758829..6b396753 100644 --- a/src/mower_comms/CMakeLists.txt +++ b/src/mower_comms_v1/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.0.2) -project(mower_comms) +project(mower_comms_v1) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) @@ -11,8 +11,8 @@ find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure mower_msgs sensor_msgs - roscpp - serial + roscpp + serial xesc_driver xesc_msgs xesc_interface @@ -113,10 +113,10 @@ find_package( ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( -# INCLUDE_DIRS include -# LIBRARIES mower_comms -# CATKIN_DEPENDS mower_msgs roscpp serial -# DEPENDS system_lib + # INCLUDE_DIRS include + # LIBRARIES mower_comms_v1 + # CATKIN_DEPENDS mower_msgs roscpp serial + # DEPENDS system_lib ) ########### @@ -126,13 +126,13 @@ catkin_package( ## Specify additional locations of header files ## Your package locations should be listed before other locations include_directories( -# include - ${catkin_INCLUDE_DIRS} + # include + ${catkin_INCLUDE_DIRS} ) ## Declare a C++ library # add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/mower_comms.cpp +# src/${PROJECT_NAME}/mower_comms_v1.cpp # ) ## Add cmake target dependencies of the library @@ -143,7 +143,7 @@ include_directories( ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/mower_comms_node.cpp) +# add_executable(${PROJECT_NAME}_node src/mower_comms_v1_node.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -160,14 +160,15 @@ include_directories( # ${catkin_LIBRARIES} # ) -add_executable(mower_comms +add_executable(mower_comms_v1 src/mower_comms.cpp src/COBS.h src/ll_datatypes.h - ) +) -add_dependencies(mower_comms ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) -target_link_libraries(mower_comms ${catkin_LIBRARIES}) +add_dependencies(mower_comms_v1 ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) +target_link_libraries(mower_comms_v1 ${catkin_LIBRARIES} +) ############# ## Install ## diff --git a/src/mower_comms/package.xml b/src/mower_comms_v1/package.xml similarity index 91% rename from src/mower_comms/package.xml rename to src/mower_comms_v1/package.xml index 3f3f9bb1..8f4116ac 100644 --- a/src/mower_comms/package.xml +++ b/src/mower_comms_v1/package.xml @@ -1,14 +1,15 @@ - mower_comms + mower_comms_v1 0.0.0 - The mower_comms package + The mower_comms_v1 package Clemens Elflein CC BY-NC-SA 4.0 https://github.com/ClemensElflein/OpenMower catkin + xbot_framework xbot_msgs mower_msgs sensor_msgs diff --git a/src/mower_comms/src/COBS.h b/src/mower_comms_v1/src/COBS.h similarity index 100% rename from src/mower_comms/src/COBS.h rename to src/mower_comms_v1/src/COBS.h diff --git a/src/mower_comms/src/ll_datatypes.h b/src/mower_comms_v1/src/ll_datatypes.h similarity index 100% rename from src/mower_comms/src/ll_datatypes.h rename to src/mower_comms_v1/src/ll_datatypes.h diff --git a/src/mower_comms/src/mower_comms.cpp b/src/mower_comms_v1/src/mower_comms.cpp similarity index 93% rename from src/mower_comms/src/mower_comms.cpp rename to src/mower_comms_v1/src/mower_comms.cpp index fc2a5e67..c6e94a7e 100644 --- a/src/mower_comms/src/mower_comms.cpp +++ b/src/mower_comms_v1/src/mower_comms.cpp @@ -18,6 +18,7 @@ // #include #include +#include #include #include #include @@ -27,6 +28,8 @@ #include #include +#include +#include #include "COBS.h" #include "boost/crc.hpp" @@ -44,6 +47,7 @@ #include "std_msgs/Empty.h" ros::Publisher status_pub; +ros::Publisher emergency_pub; ros::Publisher wheel_tick_pub; ros::Publisher sensor_imu_pub; @@ -55,6 +59,8 @@ COBS cobs; bool emergency_high_level = false; // True, if the LL board thinks there should be an emergency bool emergency_low_level = false; +// Bits are set showing which emergency is active +uint8_t active_low_level_emergency = 0; // True, if the LL emergency should be cleared in the next request bool ll_clear_emergency = false; @@ -169,6 +175,28 @@ void convertStatus(xesc_msgs::XescStateStamped &vesc_status, mower_msgs::ESCStat ros_esc_status.temperature_pcb = vesc_status.state.temperature_pcb; } +void convertStatus(xesc_msgs::XescStateStamped &vesc_status, uint8_t &esc_status, double &esc_temperature, +double &esc_current, +double &motor_temperature, +double &motor_rpm) { + if (vesc_status.state.connection_state != xesc_msgs::XescState::XESC_CONNECTION_STATE_CONNECTED && + vesc_status.state.connection_state != xesc_msgs::XescState::XESC_CONNECTION_STATE_CONNECTED_INCOMPATIBLE_FW) { + // ESC is disconnected + esc_status = mower_msgs::ESCStatus::ESC_STATUS_DISCONNECTED; + } else if (vesc_status.state.fault_code) { + ROS_ERROR_STREAM_THROTTLE(1, "Motor controller fault code: " << vesc_status.state.fault_code); + // ESC has a fault + esc_status = mower_msgs::ESCStatus::ESC_STATUS_ERROR; + } else { + // ESC is OK but standing still + esc_status = mower_msgs::ESCStatus::ESC_STATUS_OK; + } + motor_rpm = vesc_status.state.rpm; + esc_current = vesc_status.state.current_input; + motor_temperature = vesc_status.state.temperature_motor; + esc_temperature = vesc_status.state.temperature_pcb; +} + void publishStatus() { mower_msgs::Status status_msg; status_msg.stamp = ros::Time::now(); @@ -190,12 +218,9 @@ void publishStatus() { status_msg.ui_board_available = (last_ll_status.status_bitmask & 0b10000000) != 0; status_msg.mow_enabled = !(target_speed_mow == 0); - for (uint8_t i = 0; i < 5; i++) { - status_msg.ultrasonic_ranges[i] = last_ll_status.uss_ranges_m[i]; - } - // overwrite emergency with the LL value. emergency_low_level = last_ll_status.emergency_bitmask > 0; + active_low_level_emergency = last_ll_status.emergency_bitmask & 0xFE; if (!emergency_low_level) { // it obviously worked, reset the request ll_clear_emergency = false; @@ -204,11 +229,17 @@ void publishStatus() { } // True, if high or low level emergency condition is present - status_msg.emergency = is_emergency(); - - status_msg.v_battery = last_ll_status.v_system; - status_msg.v_charge = last_ll_status.v_charge; - status_msg.charge_current = last_ll_status.charging_current; + mower_msgs::Emergency emergency_msg{}; + emergency_msg.stamp = status_msg.stamp; + emergency_msg.active_emergency = active_low_level_emergency > 0; + emergency_msg.latched_emergency = is_emergency(); + emergency_msg.reason = ""; + emergency_pub.publish(emergency_msg); + + mower_msgs::Power power_msg{}; + power_msg.v_battery = last_ll_status.v_system; + power_msg.v_charge = last_ll_status.v_charge; + power_msg.charge_current = last_ll_status.charging_current; xesc_msgs::XescStateStamped mow_status, left_status, right_status; if (mow_xesc_interface) { @@ -219,9 +250,12 @@ void publishStatus() { left_xesc_interface->getStatus(left_status); right_xesc_interface->getStatus(right_status); - convertStatus(mow_status, status_msg.mow_esc_status); - convertStatus(left_status, status_msg.left_esc_status); - convertStatus(right_status, status_msg.right_esc_status); + mower_msgs::ESCStatus left_esc_status{}; + mower_msgs::ESCStatus right_esc_status{}; + + // convertStatus(mow_status, status_msg.mow_esc_status); + convertStatus(left_status, left_esc_status); + convertStatus(right_status, right_esc_status); status_pub.publish(status_msg); @@ -608,7 +642,7 @@ void reconfigCB(const mower_logic::MowerLogicConfig &config) { } int main(int argc, char **argv) { - ros::init(argc, argv, "mower_comms"); + ros::init(argc, argv, "mower_comms_v1"); sensor_mag_msg.header.seq = 0; sensor_imu_msg.header.seq = 0; @@ -657,6 +691,7 @@ int main(int argc, char **argv) { right_xesc_interface = new xesc_driver::XescDriver(n, rightParamNh); status_pub = n.advertise("mower/status", 1); + emergency_pub = n.advertise("mower/emergency", 1); wheel_tick_pub = n.advertise("mower/wheel_ticks", 1); sensor_imu_pub = n.advertise("imu/data_raw", 1); diff --git a/src/mower_comms_v2/CMakeLists.txt b/src/mower_comms_v2/CMakeLists.txt new file mode 100644 index 00000000..d82cdb57 --- /dev/null +++ b/src/mower_comms_v2/CMakeLists.txt @@ -0,0 +1,246 @@ +cmake_minimum_required(VERSION 3.16 FATAL_ERROR) +project(mower_comms_v2) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + mower_msgs + sensor_msgs + roscpp + xesc_msgs + xbot_msgs +) + +find_package( + Boost +) + + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# mower_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + # INCLUDE_DIRS include + # LIBRARIES mower_comms_v2 + # CATKIN_DEPENDS mower_msgs roscpp serial + # DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + # include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/mower_comms_v2.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/mower_comms_v2_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +add_executable(mower_comms_v2 + src/mower_comms.cpp + src/EmergencyServiceInterface.cpp + src/EmergencyServiceInterface.h + src/DiffDriveServiceInterface.cpp + src/DiffDriveServiceInterface.h + src/MowerServiceInterface.cpp + src/MowerServiceInterface.h + src/ImuServiceInterface.cpp + src/ImuServiceInterface.h + src/LidarServiceInterface.cpp + src/LidarServiceInterface.h + src/PowerServiceInterface.cpp + src/PowerServiceInterface.h + src/DockingSensorServiceInterface.cpp + src/DockingSensorServiceInterface.h + src/GpsPositionServiceInterface.cpp + src/GpsPositionServiceInterface.h +) + +target_add_service_interface(mower_comms_v2 ImuServiceInterface ${CMAKE_CURRENT_SOURCE_DIR}/../../services/imu_service.json) +target_add_service_interface(mower_comms_v2 EmergencyServiceInterface ${CMAKE_CURRENT_SOURCE_DIR}/../../services/emergency_service.json) +target_add_service_interface(mower_comms_v2 DiffDriveServiceInterface ${CMAKE_CURRENT_SOURCE_DIR}/../../services/diff_drive_service.json) +target_add_service_interface(mower_comms_v2 MowerServiceInterface ${CMAKE_CURRENT_SOURCE_DIR}/../../services/mower_service.json) +target_add_service_interface(mower_comms_v2 LidarServiceInterface ${CMAKE_CURRENT_SOURCE_DIR}/../../services/lidar_service.json) +target_add_service_interface(mower_comms_v2 PowerServiceInterface ${CMAKE_CURRENT_SOURCE_DIR}/../../services/power_service.json) +target_add_service_interface(mower_comms_v2 DockingSensorServiceInterface ${CMAKE_CURRENT_SOURCE_DIR}/../../services/docking_sensor_service.json) +target_add_service_interface(mower_comms_v2 PositionServiceInterface ${CMAKE_CURRENT_SOURCE_DIR}/../../services/position_service.json) + + +add_dependencies(mower_comms_v2 ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) +target_link_libraries(mower_comms_v2 PUBLIC ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_mower_comms.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/src/mower_comms_v2/package.xml b/src/mower_comms_v2/package.xml new file mode 100644 index 00000000..cfd53e7e --- /dev/null +++ b/src/mower_comms_v2/package.xml @@ -0,0 +1,32 @@ + + + mower_comms_v2 + 0.0.0 + The mower_comms_v2 package + Clemens Elflein + CC BY-NC-SA 4.0 + + https://github.com/ClemensElflein/OpenMower + + catkin + xbot_framework + xbot_msgs + mower_msgs + sensor_msgs + roscpp + ublox_msgs + xbot_msgs + mower_msgs + roscpp + xbot_msgs + mower_msgs + roscpp + xesc_msgs + + + + + + + + diff --git a/src/mower_comms_v2/src/DiffDriveServiceInterface.cpp b/src/mower_comms_v2/src/DiffDriveServiceInterface.cpp new file mode 100644 index 00000000..83c776f5 --- /dev/null +++ b/src/mower_comms_v2/src/DiffDriveServiceInterface.cpp @@ -0,0 +1,98 @@ +// +// Created by clemens on 25.07.24. +// + +#include "DiffDriveServiceInterface.h" + +#include +#include + +bool DiffDriveServiceInterface::OnConfigurationRequested(uint16_t service_id) { + StartTransaction(true); + SetRegisterWheelDistance(wheel_distance_); + SetRegisterWheelTicksPerMeter(ticks_per_meter_); + CommitTransaction(); + return true; +} + +void DiffDriveServiceInterface::SendTwist(const geometry_msgs::TwistConstPtr& msg) { + // Convert from ROS and publish + double data[6]{}; + data[0] = msg->linear.x; + data[1] = msg->linear.y; + data[2] = msg->linear.z; + data[3] = msg->angular.x; + data[4] = msg->angular.y; + data[5] = msg->angular.z; + SendControlTwist(data, 6); +} + +void DiffDriveServiceInterface::OnActualTwistChanged(const double* new_value, uint32_t length) { + // 3 linear, 3 angular + if (length == 6) { + // Convert to ROS and publish + geometry_msgs::TwistStamped twist; + twist.header.frame_id = "base_link"; + twist.header.stamp = ros::Time::now(); + twist.header.seq = seq++; + twist.twist.linear.x = new_value[0]; + twist.twist.linear.y = new_value[1]; + twist.twist.linear.z = new_value[2]; + twist.twist.angular.x = new_value[3]; + twist.twist.angular.y = new_value[4]; + twist.twist.angular.z = new_value[5]; + + actual_twist_publisher_.publish(twist); + } +} +void DiffDriveServiceInterface::OnLeftESCCurrentChanged(const float& new_value) { + std::unique_lock lk{state_mutex_}; + left_esc_state_.current = new_value; +} +void DiffDriveServiceInterface::OnRightESCTemperatureChanged(const float& new_value) { + std::unique_lock lk{state_mutex_}; + right_esc_state_.temperature_pcb = new_value; +} +void DiffDriveServiceInterface::OnRightESCCurrentChanged(const float& new_value) { + std::unique_lock lk{state_mutex_}; + right_esc_state_.current = new_value; +} +void DiffDriveServiceInterface::OnWheelTicksChanged(const uint32_t* new_value, uint32_t length) { + if (length != 2) return; + left_esc_state_.tacho = new_value[0]; + right_esc_state_.tacho = new_value[1]; +} +void DiffDriveServiceInterface::OnLeftESCTemperatureChanged(const float& new_value) { + std::unique_lock lk{state_mutex_}; + left_esc_state_.temperature_pcb = new_value; +} +void DiffDriveServiceInterface::OnServiceConnected(uint16_t service_id) { + std::unique_lock lk{state_mutex_}; + left_esc_state_.status = mower_msgs::ESCStatus::ESC_STATUS_DISCONNECTED; + right_esc_state_.status = mower_msgs::ESCStatus::ESC_STATUS_DISCONNECTED; +} + +void DiffDriveServiceInterface::OnLeftESCStatusChanged(const uint8_t &new_value) { + std::unique_lock lk{state_mutex_}; + left_esc_state_.status = new_value; +} + +void DiffDriveServiceInterface::OnRightESCStatusChanged(const uint8_t &new_value) { + std::unique_lock lk{state_mutex_}; + right_esc_state_.status = new_value; +} + +void DiffDriveServiceInterface::OnTransactionStart(uint64_t timestamp) { +} + +void DiffDriveServiceInterface::OnServiceDisconnected(uint16_t service_id) { + std::unique_lock lk{state_mutex_}; + left_esc_state_.status = mower_msgs::ESCStatus::ESC_STATUS_DISCONNECTED; + right_esc_state_.status = mower_msgs::ESCStatus::ESC_STATUS_DISCONNECTED; +} + +void DiffDriveServiceInterface::OnTransactionEnd() { + // Publish values to ROS + left_esc_status_publisher_.publish(left_esc_state_); + right_esc_status_publisher_.publish(right_esc_state_); +} diff --git a/src/mower_comms_v2/src/DiffDriveServiceInterface.h b/src/mower_comms_v2/src/DiffDriveServiceInterface.h new file mode 100644 index 00000000..3971608b --- /dev/null +++ b/src/mower_comms_v2/src/DiffDriveServiceInterface.h @@ -0,0 +1,76 @@ +// +// Created by clemens on 25.07.24. +// + +#ifndef DIFFDRIVESERVICEINTERFACE_H +#define DIFFDRIVESERVICEINTERFACE_H + +#include +#include +#include +#include + +#include + +class DiffDriveServiceInterface : public DiffDriveServiceInterfaceBase { + public: + DiffDriveServiceInterface(uint16_t service_id, const xbot::serviceif::Context& ctx, + const ros::Publisher& actual_twist_publisher, + const ros::Publisher& left_esc_status_publisher, + const ros::Publisher& right_esc_status_publisher, double ticks_per_meter, + double wheel_distance) + : DiffDriveServiceInterfaceBase(service_id, ctx), + actual_twist_publisher_(actual_twist_publisher), + left_esc_status_publisher_(left_esc_status_publisher), + right_esc_status_publisher_(right_esc_status_publisher), + wheel_distance_(wheel_distance), + ticks_per_meter_(ticks_per_meter) { + } + + bool OnConfigurationRequested(uint16_t service_id) override; + + /** + * Convenience function to transmit the twist from a ROS message + * @param msg The ROS message + */ + void SendTwist(const geometry_msgs::TwistConstPtr& msg); + + protected: + /** + * Callback whenever an updated twist arrives + * @param new_value the updated value + * @param length length of array + */ + void OnActualTwistChanged(const double* new_value, uint32_t length) override; + void OnLeftESCTemperatureChanged(const float& new_value) override; + void OnLeftESCCurrentChanged(const float& new_value) override; + void OnRightESCTemperatureChanged(const float& new_value) override; + void OnRightESCCurrentChanged(const float& new_value) override; + void OnWheelTicksChanged(const uint32_t* new_value, uint32_t length) override; + void OnLeftESCStatusChanged(const uint8_t &new_value) override; + void OnRightESCStatusChanged(const uint8_t &new_value) override; + + private: + void OnServiceConnected(uint16_t service_id) override; + void OnTransactionStart(uint64_t timestamp) override; + void OnTransactionEnd() override; + void OnServiceDisconnected(uint16_t service_id) override; + + private: + // Store the seq number for the actual twist message + uint32_t seq = 0; + std::mutex state_mutex_{}; + + public: + const ros::Publisher& actual_twist_publisher_; + const ros::Publisher& left_esc_status_publisher_; + const ros::Publisher& right_esc_status_publisher_; + double wheel_distance_; + double ticks_per_meter_; + + // Store the latest ESC state + mower_msgs::ESCStatus left_esc_state_{}; + mower_msgs::ESCStatus right_esc_state_{}; +}; + +#endif // DIFFDRIVESERVICEINTERFACE_H diff --git a/src/mower_comms_v2/src/DockingSensorServiceInterface.cpp b/src/mower_comms_v2/src/DockingSensorServiceInterface.cpp new file mode 100644 index 00000000..a3675645 --- /dev/null +++ b/src/mower_comms_v2/src/DockingSensorServiceInterface.cpp @@ -0,0 +1,26 @@ +// +// Created by clemens on 09.09.24. +// + +#include "DockingSensorServiceInterface.h" +bool DockingSensorServiceInterface::OnConfigurationRequested(uint16_t service_id) { + StartTransaction(true); + SetRegisterSensorTimeoutMs(1000); + CommitTransaction(); + return true; +} +void DockingSensorServiceInterface::OnDetectedSensorsLeftChanged(const uint8_t& new_value) { + msg.detected_left = new_value; +} +void DockingSensorServiceInterface::OnDetectedSensorsRightChanged(const uint8_t& new_value) { + msg.detected_right = new_value; +} +void DockingSensorServiceInterface::OnServiceConnected(uint16_t service_id) { +} +void DockingSensorServiceInterface::OnTransactionStart(uint64_t timestamp) { + msg = {}; + msg.stamp = ros::Time::now(); +} +void DockingSensorServiceInterface::OnTransactionEnd() { + sensor_publisher_.publish(msg); +} diff --git a/src/mower_comms_v2/src/DockingSensorServiceInterface.h b/src/mower_comms_v2/src/DockingSensorServiceInterface.h new file mode 100644 index 00000000..35640b5f --- /dev/null +++ b/src/mower_comms_v2/src/DockingSensorServiceInterface.h @@ -0,0 +1,33 @@ +// +// Created by clemens on 09.09.24. +// + +#ifndef DOCKINGSENSORSERVICEINTERFACE_H +#define DOCKINGSENSORSERVICEINTERFACE_H + +#include +#include + +#include + +class DockingSensorServiceInterface : public DockingSensorServiceInterfaceBase { + public: + DockingSensorServiceInterface(uint16_t service_id, const xbot::serviceif::Context& ctx, + const ros::Publisher& sensor_publisher) + : DockingSensorServiceInterfaceBase(service_id, ctx), sensor_publisher_(sensor_publisher) { + } + bool OnConfigurationRequested(uint16_t service_id) override; + + protected: + void OnDetectedSensorsLeftChanged(const uint8_t& new_value) override; + void OnDetectedSensorsRightChanged(const uint8_t& new_value) override; + + private: + void OnServiceConnected(uint16_t service_id) override; + void OnTransactionStart(uint64_t timestamp) override; + void OnTransactionEnd() override; + const ros::Publisher& sensor_publisher_; + mower_msgs::DockingSensor msg{}; +}; + +#endif // DOCKINGSENSORSERVICEINTERFACE_H diff --git a/src/mower_comms_v2/src/EmergencyServiceInterface.cpp b/src/mower_comms_v2/src/EmergencyServiceInterface.cpp new file mode 100644 index 00000000..39e6b537 --- /dev/null +++ b/src/mower_comms_v2/src/EmergencyServiceInterface.cpp @@ -0,0 +1,82 @@ +// +// Created by clemens on 25.07.24. +// + +#include "EmergencyServiceInterface.h" + +#include + +bool EmergencyServiceInterface::SetEmergency(bool new_value) { + std::unique_lock lk{state_mutex_}; + // Set the high level emergency + active_high_level_emergency_ = new_value; + + if (new_value) { + latest_emergency_reason_ = "High Level Emergency"; + } + + // Send the new emergency state to the service. + // the LL emergency will be updated in the callback + SendSetEmergency(new_value); + + // Instantly send the new emergency + PublishEmergencyState(); + return true; +} +void EmergencyServiceInterface::Heartbeat() { + SendSetEmergency(latched_emergency_); +} + +bool EmergencyServiceInterface::OnConfigurationRequested(uint16_t service_id) { + // No config needed + return true; +} +void EmergencyServiceInterface::OnEmergencyActiveChanged(const uint8_t& new_value) { + std::unique_lock lk{state_mutex_}; + active_low_level_emergency_ = new_value; +} + +void EmergencyServiceInterface::OnEmergencyLatchChanged(const uint8_t& new_value) { + std::unique_lock lk{state_mutex_}; + if (new_value) { + // If there is a new emergency, we set it also in the high level + latched_emergency_ = true; + } else if (!active_high_level_emergency_) { + // Only clear high level latch, if we don't have a high level emergency going on + latched_emergency_ = false; + } +} +void EmergencyServiceInterface::OnEmergencyReasonChanged(const char* new_value, uint32_t length) { + latest_emergency_reason_ = std::string{new_value, length}; +} +void EmergencyServiceInterface::OnTransactionStart(uint64_t timestamp) { +} +void EmergencyServiceInterface::OnTransactionEnd() { + // Send the packet + PublishEmergencyState(); +} + +void EmergencyServiceInterface::OnServiceConnected(uint16_t service_id) { + std::unique_lock lk{state_mutex_}; + latched_emergency_ = active_high_level_emergency_ = active_low_level_emergency_ = true; + latest_emergency_reason_ = "Service Starting Up"; + PublishEmergencyState(); +} +void EmergencyServiceInterface::OnServiceDisconnected(uint16_t service_id) { + std::unique_lock lk{state_mutex_}; + latched_emergency_ = active_high_level_emergency_ = active_low_level_emergency_ = true; + latest_emergency_reason_ = "Service Disconnected"; + PublishEmergencyState(); +} +void EmergencyServiceInterface::PublishEmergencyState() { + mower_msgs::Emergency emergency_{}; + std::unique_lock lk{state_mutex_}; + emergency_.stamp = ros::Time::now(); + // Make sure the latch is set, if there's an active emergency + latched_emergency_ |= active_high_level_emergency_ | active_low_level_emergency_; + + emergency_.latched_emergency = latched_emergency_; + emergency_.active_emergency = active_high_level_emergency_ | active_low_level_emergency_; + emergency_.reason = latest_emergency_reason_; + publisher.publish(emergency_); +} diff --git a/src/mower_comms_v2/src/EmergencyServiceInterface.h b/src/mower_comms_v2/src/EmergencyServiceInterface.h new file mode 100644 index 00000000..557aaa68 --- /dev/null +++ b/src/mower_comms_v2/src/EmergencyServiceInterface.h @@ -0,0 +1,48 @@ +// +// Created by clemens on 25.07.24. +// + +#ifndef EMERGENCYSERVICEINTERFACE_H +#define EMERGENCYSERVICEINTERFACE_H + +#include + +#include + +namespace sc = std::chrono; + +class EmergencyServiceInterface : public EmergencyServiceInterfaceBase { + public: + EmergencyServiceInterface(uint16_t service_id, const xbot::serviceif::Context& ctx, const ros::Publisher& publisher) + : EmergencyServiceInterfaceBase(service_id, ctx), publisher(publisher) { + } + + bool SetEmergency(bool new_value); + void Heartbeat(); + + protected: + bool OnConfigurationRequested(uint16_t service_id) override; + void OnEmergencyActiveChanged(const uint8_t& new_value) override; + void OnEmergencyLatchChanged(const uint8_t& new_value) override; + void OnEmergencyReasonChanged(const char* new_value, uint32_t length) override; + + private: + void OnServiceConnected(uint16_t service_id) override; + void OnTransactionStart(uint64_t timestamp) override; + void OnTransactionEnd() override; + void OnServiceDisconnected(uint16_t service_id) override; + + void PublishEmergencyState(); + + std::recursive_mutex state_mutex_{}; + + const ros::Publisher& publisher; + + // keep track of high level emergency + bool latched_emergency_ = true; + bool active_low_level_emergency_ = true; + bool active_high_level_emergency_ = true; + std::string latest_emergency_reason_ = "NONE"; +}; + +#endif // EMERGENCYSERVICEINTERFACE_H diff --git a/src/mower_comms_v2/src/GpsPositionServiceInterface.cpp b/src/mower_comms_v2/src/GpsPositionServiceInterface.cpp new file mode 100644 index 00000000..0aacc081 --- /dev/null +++ b/src/mower_comms_v2/src/GpsPositionServiceInterface.cpp @@ -0,0 +1,86 @@ +// +// Created by clemens on 30.11.24. +// + +#include "GpsPositionServiceInterface.h" +bool GpsPositionServiceInterface::OnConfigurationRequested(uint16_t service_id) { + StartTransaction(true); + SetRegisterBaudrate(921600); + SetRegisterProtocol("UBX", 3); + double llh[3]{}; + SetRegisterDatumLatLonHeight(llh,3); + CommitTransaction(); + return true; +} + + +void GpsPositionServiceInterface::OnTransactionStart(uint64_t timestamp) { + pose_msg_.header.frame_id = "gps"; + pose_msg_.header.stamp = ros::Time::now(); + pose_msg_.header.seq++; + pose_msg_.motion_vector_valid = false; + pose_msg_.orientation_valid = false; + pose_msg_.sensor_stamp = timestamp / 1000; + pose_msg_.flags = 0; +} + + +void GpsPositionServiceInterface::OnPositionXYZChanged(const double* new_value, uint32_t length) { + if(length != 3) { + ROS_INFO_STREAM("OnPositionXYZChanged called with length " << length); + return; + } + pose_msg_.pose.pose.position.x = new_value[0]; + pose_msg_.pose.pose.position.y = new_value[1]; + pose_msg_.pose.pose.position.z = new_value[2]; +} +void GpsPositionServiceInterface::OnPositionAccuracyChanged(const double& new_value) { + pose_msg_.position_accuracy = static_cast(new_value); +} +void GpsPositionServiceInterface::OnFixTypeChanged(const char* new_value, uint32_t length) { + pose_msg_.flags |= xbot_msgs::AbsolutePose::FLAG_GPS_RTK; + std::string type(new_value, length); + if(type == "FIX") { + pose_msg_.flags |= xbot_msgs::AbsolutePose::FLAG_GPS_RTK_FIXED; + } else if(type == "FLOAT") { + pose_msg_.flags |= xbot_msgs::AbsolutePose::FLAG_GPS_RTK_FLOAT; + } +} +void GpsPositionServiceInterface::OnMotionVectorXYZChanged(const double* new_value, uint32_t length) { + if(length != 3) { + ROS_INFO_STREAM("OnMotionVectorXYZChanged called with length " << length); + return; + } + pose_msg_.motion_vector_valid = true; + pose_msg_.motion_vector.x = new_value[0]; + pose_msg_.motion_vector.y = new_value[1]; + pose_msg_.motion_vector.z = new_value[2]; +} +void GpsPositionServiceInterface::OnMotionHeadingAndAccuracyChanged(const double* new_value, uint32_t length) { + if(length != 2) { + ROS_INFO_STREAM("OnMotionHeadingAndAccuracyChanged called with length " << length); + return; + } + pose_msg_.motion_heading = new_value[0]; + pose_msg_.motion_vector_valid = true; +} +void GpsPositionServiceInterface::OnVehicleHeadingAndAccuracyChanged(const double* new_value, uint32_t length) { + if(length != 2) { + ROS_INFO_STREAM("OnVehicleHeadingAndAccuracyChanged called with length " << length); + return; + } + pose_msg_.vehicle_heading = new_value[0]; + pose_msg_.orientation_accuracy = new_value[1]; + pose_msg_.orientation_valid = true; +} +void GpsPositionServiceInterface::OnNMEAChanged(const char* new_value, uint32_t length) { + (void)new_value; + (void)length; +} +void GpsPositionServiceInterface::OnServiceConnected(uint16_t service_id) { +} +void GpsPositionServiceInterface::OnTransactionEnd() { + absolute_pose_publisher_.publish(pose_msg_); +} +void GpsPositionServiceInterface::OnServiceDisconnected(uint16_t service_id) { +} diff --git a/src/mower_comms_v2/src/GpsPositionServiceInterface.h b/src/mower_comms_v2/src/GpsPositionServiceInterface.h new file mode 100644 index 00000000..ae4ea51d --- /dev/null +++ b/src/mower_comms_v2/src/GpsPositionServiceInterface.h @@ -0,0 +1,40 @@ +// +// Created by clemens on 30.11.24. +// + +#ifndef GPSPOSITIONSERVICEINTERFACE_H +#define GPSPOSITIONSERVICEINTERFACE_H +#include +#include + +#include + + +class GpsPositionServiceInterface : public PositionServiceInterfaceBase { +public: + GpsPositionServiceInterface(uint16_t service_id, const xbot::serviceif::Context& ctx, const ros::Publisher& imu_publisher) + : PositionServiceInterfaceBase(service_id, ctx), absolute_pose_publisher_(imu_publisher) { + } + bool OnConfigurationRequested(uint16_t service_id) override; + +protected: + void OnPositionXYZChanged(const double* new_value, uint32_t length) override; + void OnPositionAccuracyChanged(const double& new_value) override; + void OnFixTypeChanged(const char* new_value, uint32_t length) override; + void OnMotionVectorXYZChanged(const double* new_value, uint32_t length) override; + void OnMotionHeadingAndAccuracyChanged(const double* new_value, uint32_t length) override; + void OnVehicleHeadingAndAccuracyChanged(const double* new_value, uint32_t length) override; + void OnNMEAChanged(const char* new_value, uint32_t length) override; + + private: + void OnServiceConnected(uint16_t service_id) override; + void OnTransactionStart(uint64_t timestamp) override; + void OnTransactionEnd() override; + void OnServiceDisconnected(uint16_t service_id) override; + const ros::Publisher& absolute_pose_publisher_; + xbot_msgs::AbsolutePose pose_msg_; +}; + + + +#endif //GPSPOSITIONSERVICEINTERFACE_H diff --git a/src/mower_comms_v2/src/ImuServiceInterface.cpp b/src/mower_comms_v2/src/ImuServiceInterface.cpp new file mode 100644 index 00000000..78b71509 --- /dev/null +++ b/src/mower_comms_v2/src/ImuServiceInterface.cpp @@ -0,0 +1,32 @@ +// +// Created by clemens on 26.07.24. +// + +#include "ImuServiceInterface.h" + +bool ImuServiceInterface::OnConfigurationRequested(uint16_t service_id) { + return true; +} +void ImuServiceInterface::OnAxesChanged(const double* new_value, uint32_t length) { + if (length < 6) { + return; + } + imu_msg.header.stamp = ros::Time::now(); + imu_msg.header.seq++; + imu_msg.header.frame_id = "base_link"; + imu_msg.linear_acceleration.x = new_value[0]; + imu_msg.linear_acceleration.y = new_value[1]; + imu_msg.linear_acceleration.z = new_value[2]; + imu_msg.angular_velocity.x = new_value[3]; + imu_msg.angular_velocity.y = new_value[4]; + imu_msg.angular_velocity.z = new_value[5]; + imu_publisher_.publish(imu_msg); +} +void ImuServiceInterface::OnServiceConnected(uint16_t service_id) { +} +void ImuServiceInterface::OnTransactionStart(uint64_t timestamp) { +} +void ImuServiceInterface::OnTransactionEnd() { +} +void ImuServiceInterface::OnServiceDisconnected(uint16_t service_id) { +} diff --git a/src/mower_comms_v2/src/ImuServiceInterface.h b/src/mower_comms_v2/src/ImuServiceInterface.h new file mode 100644 index 00000000..2b84f6f3 --- /dev/null +++ b/src/mower_comms_v2/src/ImuServiceInterface.h @@ -0,0 +1,33 @@ +// +// Created by clemens on 26.07.24. +// + +#ifndef IMUSERVICEINTERFACE_H +#define IMUSERVICEINTERFACE_H + +#include +#include + +#include + +class ImuServiceInterface : public ImuServiceInterfaceBase { + public: + ImuServiceInterface(uint16_t service_id, const xbot::serviceif::Context& ctx, const ros::Publisher& imu_publisher) + : ImuServiceInterfaceBase(service_id, ctx), imu_publisher_(imu_publisher) { + } + bool OnConfigurationRequested(uint16_t service_id) override; + + protected: + void OnAxesChanged(const double* new_value, uint32_t length) override; + + private: + void OnServiceConnected(uint16_t service_id) override; + void OnTransactionStart(uint64_t timestamp) override; + void OnTransactionEnd() override; + void OnServiceDisconnected(uint16_t service_id) override; + const ros::Publisher& imu_publisher_; + + sensor_msgs::Imu imu_msg{}; +}; + +#endif // IMUSERVICEINTERFACE_H diff --git a/src/mower_comms_v2/src/LidarServiceInterface.cpp b/src/mower_comms_v2/src/LidarServiceInterface.cpp new file mode 100644 index 00000000..33ee464d --- /dev/null +++ b/src/mower_comms_v2/src/LidarServiceInterface.cpp @@ -0,0 +1,131 @@ +// +// Created by clemens on 28.08.24. +// + +#include "LidarServiceInterface.h" + +#include "spdlog/spdlog.h" + +bool LidarServiceInterface::OnConfigurationRequested(uint16_t service_id) { + return true; +} + +void LidarServiceInterface::OnAngleMinRadChanged(const double &new_value) { + angle_min = new_value; +} + +void LidarServiceInterface::OnAngleMaxRadChanged(const double &new_value) { + angle_max = new_value; +} + +void LidarServiceInterface::OnTimeIncrementSecondsChanged(const float &new_value) { + time_increment = new_value; +} + +void LidarServiceInterface::OnScanTimeSecondsChanged(const float &new_value) { + scan_time = new_value; +} + +void LidarServiceInterface::OnRangeMinMChanged(const float &new_value) { + min_m = std::min(min_m, new_value); +} + +void LidarServiceInterface::OnRangeMaxMChanged(const float &new_value) { + max_m = std::max(max_m, new_value); +} + +void LidarServiceInterface::OnRangesChanged(const float *new_value, uint32_t length) { + ranges.clear(); + ranges.insert(ranges.end(), new_value, new_value + length); +} + +void LidarServiceInterface::OnIntensitiesChanged(const float *new_value, uint32_t length) { +} + +void LidarServiceInterface::OnNewScanChanged(const uint8_t &new_value) { + new_scan = new_value; +} + +void LidarServiceInterface::OnTransactionEnd() { + if (new_scan) { + // append ranges from the old rotation + { + float current_angle_increment = fmodf(angle_max - angle_min + 2.0 * M_PI, 2.0 * M_PI) / ranges.size(); + while (angle_min > M_PI && !ranges.empty()) { + if (laser_scan_msg_.ranges.size() < 400) { + laser_scan_msg_.ranges.push_back(ranges.front()); + } + ranges.pop_front(); + laser_scan_msg_.angle_max = angle_min; + angle_min = fmodf(angle_min + current_angle_increment, M_PI * 2.0); + } + } + + // Send the gathered data + const auto now = ros::Time::now(); + if (!laser_scan_msg_.ranges.empty()) { + if (last_full_scan_time_.is_zero()) { + last_full_scan_time_ = now; + } else { + { + // some SLAM algorithm want a constant amount of points, otherwise it rejects the laser scan.... + // our laser theoretically gives 400 pts, but sometimes its +- a few, so yea.. + while (laser_scan_msg_.ranges.size() > 400) { + laser_scan_msg_.ranges.pop_back(); + } + while (laser_scan_msg_.ranges.size() < 400) { + laser_scan_msg_.ranges.push_back(0); + } + } + + laser_scan_msg_.header.frame_id = "lidar"; + laser_scan_msg_.header.seq++; + laser_scan_msg_.range_min = 0.0; + laser_scan_msg_.range_max = 20.0; + laser_scan_msg_.scan_time = (now - last_full_scan_time_).toSec(); + laser_scan_msg_.time_increment = laser_scan_msg_.scan_time / laser_scan_msg_.ranges.size(); + + + // our laser scanner does sometimes report weird start/end angles, so we set min-max angles manually + // TODO: either fix, or make configurable + laser_scan_msg_.angle_max = 2.0 * M_PI - M_PI / 800; + laser_scan_msg_.angle_min = 0; + // END: fix + + laser_scan_msg_.angle_increment = + fmodf(laser_scan_msg_.angle_max - laser_scan_msg_.angle_min + 2.0 * M_PI, 2.0 * M_PI) / + laser_scan_msg_.ranges.size(); + + last_full_scan_time_ = now; + + // TODO: copy needed? + sensor_msgs::LaserScan copy{laser_scan_msg_}; + lidar_publisher_.publish(copy); + } + } + new_scan = false; + + // Set the data from the last transaction as a start + laser_scan_msg_.header.stamp = now - ros::Duration().fromNSec(time_offset_micros * 1000); + laser_scan_msg_.angle_min = angle_min; + laser_scan_msg_.angle_max = angle_max; + laser_scan_msg_.ranges.clear(); + laser_scan_msg_.ranges.insert(laser_scan_msg_.ranges.end(), ranges.begin(), ranges.end()); + laser_scan_msg_.range_max = max_m; + laser_scan_msg_.range_min = min_m; + } else { + // append to the scan + laser_scan_msg_.range_max = std::max(laser_scan_msg_.range_max, max_m); + laser_scan_msg_.range_min = std::min(laser_scan_msg_.range_min, min_m); + + laser_scan_msg_.angle_max = angle_max; + // keep it to a sane value in case the new_scan flag is not sent correctly + if (laser_scan_msg_.ranges.size() < 400) { + laser_scan_msg_.ranges.insert(laser_scan_msg_.ranges.end(), ranges.begin(), ranges.end()); + } + } +} + +void LidarServiceInterface::OnTransactionStart(uint64_t timestamp) { + time_offset_micros = timestamp; +} diff --git a/src/mower_comms_v2/src/LidarServiceInterface.h b/src/mower_comms_v2/src/LidarServiceInterface.h new file mode 100644 index 00000000..85988f68 --- /dev/null +++ b/src/mower_comms_v2/src/LidarServiceInterface.h @@ -0,0 +1,60 @@ +// +// Created by clemens on 28.08.24. +// + +#ifndef LIDARSERVICEINTERFACE_H +#define LIDARSERVICEINTERFACE_H + +#include +#include + +#include + +class LidarServiceInterface : public LidarServiceInterfaceBase { +public: + LidarServiceInterface(uint16_t service_id, const xbot::serviceif::Context &ctx, const ros::Publisher &lidar_publisher) + : LidarServiceInterfaceBase(service_id, ctx), lidar_publisher_(lidar_publisher) { + } + + bool OnConfigurationRequested(uint16_t service_id) override; + +protected: + void OnAngleMinRadChanged(const double &new_value) override; + + void OnAngleMaxRadChanged(const double &new_value) override; + + void OnTimeIncrementSecondsChanged(const float &new_value) override; + + void OnScanTimeSecondsChanged(const float &new_value) override; + + void OnRangeMinMChanged(const float &new_value) override; + + void OnRangeMaxMChanged(const float &new_value) override; + + void OnRangesChanged(const float *new_value, uint32_t length) override; + + void OnIntensitiesChanged(const float *new_value, uint32_t length) override; + + void OnNewScanChanged(const uint8_t &new_value) override; + +private: + void OnTransactionEnd() override; + + void OnTransactionStart(uint64_t timestamp) override; + + const ros::Publisher &lidar_publisher_; + std::deque ranges; + float max_m = 0; + float min_m = 999; + float scan_time = 0; + float time_increment = 0; + float angle_min = 0; + float angle_max = 0; + bool new_scan = false; + uint64_t time_offset_micros = 0; + ros::Time last_full_scan_time_{0}; + + sensor_msgs::LaserScan laser_scan_msg_{}; +}; + +#endif // LIDARSERVICEINTERFACE_H diff --git a/src/mower_comms_v2/src/MowerServiceInterface.cpp b/src/mower_comms_v2/src/MowerServiceInterface.cpp new file mode 100644 index 00000000..62c04e68 --- /dev/null +++ b/src/mower_comms_v2/src/MowerServiceInterface.cpp @@ -0,0 +1,45 @@ +// +// Created by clemens on 25.07.24. +// + +#include "MowerServiceInterface.h" + +bool MowerServiceInterface::OnConfigurationRequested(uint16_t service_id) { + // No configuration + return true; +} +void MowerServiceInterface::SetMowerEnabled(bool enabled) { + SendMowerEnabled(enabled); +} +void MowerServiceInterface::OnMowerStatusChanged(const uint8_t& new_value) { + status_msg_.mower_status = new_value; +} +void MowerServiceInterface::OnRainDetectedChanged(const uint8_t& new_value) { + status_msg_.rain_detected = new_value; +} +void MowerServiceInterface::OnMowerRunningChanged(const uint8_t& new_value) { + status_msg_.mow_enabled = new_value; +} +void MowerServiceInterface::OnMowerESCTemperatureChanged(const float& new_value) { + status_msg_.mower_esc_temperature = new_value; +} +void MowerServiceInterface::OnMowerMotorTemperatureChanged(const float& new_value) { + status_msg_.mower_motor_temperature = new_value; +} +void MowerServiceInterface::OnMowerMotorCurrentChanged(const float& new_value) { + status_msg_.mower_esc_current = new_value; +} +void MowerServiceInterface::OnMowerMotorRPMChanged(const float& new_value) { + status_msg_.mower_motor_rpm = new_value; +} +void MowerServiceInterface::OnServiceConnected(uint16_t service_id) { + status_msg_ = {}; +} +void MowerServiceInterface::OnTransactionStart(uint64_t timestamp) { + status_msg_.stamp = ros::Time::now(); +} +void MowerServiceInterface::OnTransactionEnd() { + status_publisher_.publish(status_msg_); +} +void MowerServiceInterface::OnServiceDisconnected(uint16_t service_id) { +} diff --git a/src/mower_comms_v2/src/MowerServiceInterface.h b/src/mower_comms_v2/src/MowerServiceInterface.h new file mode 100644 index 00000000..d96d15d1 --- /dev/null +++ b/src/mower_comms_v2/src/MowerServiceInterface.h @@ -0,0 +1,43 @@ +// +// Created by clemens on 25.07.24. +// + +#ifndef MOWERSERVICEINTERFACE_H +#define MOWERSERVICEINTERFACE_H + +#include +#include + +#include + +class MowerServiceInterface : public MowerServiceInterfaceBase { + public: + MowerServiceInterface(uint16_t service_id, const xbot::serviceif::Context& ctx, + const ros::Publisher& status_publisher) + : MowerServiceInterfaceBase(service_id, ctx), status_publisher_(status_publisher) { + } + + bool OnConfigurationRequested(uint16_t service_id) override; + void SetMowerEnabled(bool enabled); + + protected: + void OnMowerStatusChanged(const uint8_t& new_value) override; + void OnRainDetectedChanged(const uint8_t& new_value) override; + void OnMowerRunningChanged(const uint8_t& new_value) override; + void OnMowerESCTemperatureChanged(const float& new_value) override; + void OnMowerMotorTemperatureChanged(const float& new_value) override; + void OnMowerMotorCurrentChanged(const float& new_value) override; + void OnMowerMotorRPMChanged(const float& new_value) override; + + private: + void OnServiceConnected(uint16_t service_id) override; + void OnTransactionStart(uint64_t timestamp) override; + void OnTransactionEnd() override; + void OnServiceDisconnected(uint16_t service_id) override; + + private: + mower_msgs::Status status_msg_{}; + const ros::Publisher& status_publisher_; +}; + +#endif // MOWERSERVICEINTERFACE_H diff --git a/src/mower_comms_v2/src/PowerServiceInterface.cpp b/src/mower_comms_v2/src/PowerServiceInterface.cpp new file mode 100644 index 00000000..ea8b9ec8 --- /dev/null +++ b/src/mower_comms_v2/src/PowerServiceInterface.cpp @@ -0,0 +1,31 @@ +// +// Created by clemens on 09.09.24. +// + +#include "PowerServiceInterface.h" +bool PowerServiceInterface::OnConfigurationRequested(uint16_t service_id) { + return true; +} +void PowerServiceInterface::OnChargeVoltageChanged(const float& new_value) { + power_msg_.v_charge = new_value; +} +void PowerServiceInterface::OnChargeCurrentChanged(const float& new_value) { + power_msg_.charge_current = new_value; +} +void PowerServiceInterface::OnBatteryVoltageChanged(const float& new_value) { + power_msg_.v_battery = new_value; +} +void PowerServiceInterface::OnChargingStatusChanged(const char* new_value, uint32_t length) { + power_msg_.charger_status = std::string(new_value, length); +} +void PowerServiceInterface::OnChargerEnabledChanged(const uint8_t& new_value) { + power_msg_.charger_enabled = new_value; +} +void PowerServiceInterface::OnTransactionStart(uint64_t timestamp) { + power_msg_ = {}; + power_msg_.stamp = ros::Time::now(); +} + +void PowerServiceInterface::OnTransactionEnd() { + status_publisher_.publish(power_msg_); +} diff --git a/src/mower_comms_v2/src/PowerServiceInterface.h b/src/mower_comms_v2/src/PowerServiceInterface.h new file mode 100644 index 00000000..1b36e69d --- /dev/null +++ b/src/mower_comms_v2/src/PowerServiceInterface.h @@ -0,0 +1,35 @@ +// +// Created by clemens on 09.09.24. +// + +#ifndef POWERSERVICEINTERFACE_H +#define POWERSERVICEINTERFACE_H + +#include +#include + +#include + +class PowerServiceInterface : public PowerServiceInterfaceBase { + public: + PowerServiceInterface(uint16_t service_id, const xbot::serviceif::Context& ctx, + const ros::Publisher& status_publisher) + : PowerServiceInterfaceBase(service_id, ctx), status_publisher_(status_publisher) { + } + bool OnConfigurationRequested(uint16_t service_id) override; + + protected: + void OnChargeVoltageChanged(const float& new_value) override; + void OnChargeCurrentChanged(const float& new_value) override; + void OnBatteryVoltageChanged(const float& new_value) override; + void OnChargingStatusChanged(const char* new_value, uint32_t length) override; + void OnChargerEnabledChanged(const uint8_t& new_value) override; + + private: + void OnTransactionStart(uint64_t timestamp) override; + void OnTransactionEnd() override; + mower_msgs::Power power_msg_{}; + const ros::Publisher& status_publisher_; +}; + +#endif // POWERSERVICEINTERFACE_H diff --git a/src/mower_comms_v2/src/mower_comms.cpp b/src/mower_comms_v2/src/mower_comms.cpp new file mode 100644 index 00000000..0cc23e40 --- /dev/null +++ b/src/mower_comms_v2/src/mower_comms.cpp @@ -0,0 +1,146 @@ +// +// Created by Clemens Elflein on 15.03.22. +// Copyright (c) 2022 Clemens Elflein. All rights reserved. +// +// This work is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License. +// +// Feel free to use the design in your private/educational projects, but don't try to sell the design or products based +// on it without getting my consent first. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +// +#include +#include +#include +#include +#include +#include +#include +#include + +#include "DiffDriveServiceInterface.h" +#include "DockingSensorServiceInterface.h" +#include "EmergencyServiceInterface.h" +#include "GpsPositionServiceInterface.h" +#include "ImuServiceInterface.h" +#include "LidarServiceInterface.h" +#include "MowerServiceInterface.h" +#include "PowerServiceInterface.h" + +ros::Publisher status_pub; +ros::Publisher power_pub; +ros::Publisher gps_position_pub; +ros::Publisher status_left_esc_pub; +ros::Publisher status_right_esc_pub; +ros::Publisher emergency_pub; +ros::Publisher actual_twist_pub; + +ros::Publisher sensor_imu_pub; +ros::Publisher sensor_lidar_pub; +ros::Publisher sensor_dock_pub; + +ros::ServiceClient highLevelClient; + +std::unique_ptr emergency_service = nullptr; +std::unique_ptr diff_drive_service = nullptr; +std::unique_ptr mower_service = nullptr; +std::unique_ptr imu_service = nullptr; +std::unique_ptr lidar_service = nullptr; +std::unique_ptr power_service = nullptr; +std::unique_ptr docking_sensor_service = nullptr; +std::unique_ptr gps_position_service = nullptr; + +xbot::serviceif::Context ctx{}; + +bool setEmergencyStop(mower_msgs::EmergencyStopSrvRequest &req, mower_msgs::EmergencyStopSrvResponse &res) { + // This should never be the case, also this is no race condition, because callback will only be called + // after initialization whereas the service is created during intialization + if (!emergency_service) return false; + + emergency_service->SetEmergency(req.emergency); + return true; +} + +void velReceived(const geometry_msgs::Twist::ConstPtr &msg) { + if (!diff_drive_service) return; + diff_drive_service->SendTwist(msg); +} + +void sendEmergencyHeartbeatTimerTask(const ros::TimerEvent &) { + emergency_service->Heartbeat(); +} + +bool setMowEnabled(mower_msgs::MowerControlSrvRequest &req, mower_msgs::MowerControlSrvRequest &res) { + mower_service->SetMowerEnabled(req.mow_enabled); + return true; +} + +int main(int argc, char **argv) { + ros::init(argc, argv, "mower_comms_v2"); + + ros::NodeHandle n; + ros::NodeHandle paramNh("~"); + + highLevelClient = n.serviceClient("mower_service/high_level_control"); + + // Ticks / m and wheel distance for this robot + double wheel_ticks_per_m = 0.0; + double wheel_distance_m = 0.0; + + std::string bind_ip = "0.0.0.0"; + paramNh.getParam("bind_ip", bind_ip); + paramNh.getParam("wheel_ticks_per_m", wheel_ticks_per_m); + paramNh.getParam("wheel_distance_m", wheel_distance_m); + + ROS_INFO_STREAM("Bind IP (Robot Internal): " << bind_ip); + ROS_INFO_STREAM("Wheel ticks [1/m]: " << wheel_ticks_per_m); + ROS_INFO_STREAM("Wheel distance [m]: " << wheel_distance_m); + + status_pub = n.advertise("ll/mower_status", 1); + status_left_esc_pub = n.advertise("ll/diff_drive/left_esc_status", 1); + status_right_esc_pub = n.advertise("ll/diff_drive/right_esc_status", 1); + emergency_pub = n.advertise("ll/emergency", 1); + actual_twist_pub = n.advertise("ll/diff_drive/measured_twist", 1); + sensor_imu_pub = n.advertise("ll/imu/data_raw", 1); + sensor_dock_pub = n.advertise("ll/dock_sensor", 1); + sensor_lidar_pub = n.advertise("ll/lidar", 1); + power_pub = n.advertise("ll/power", 1); + gps_position_pub = n.advertise("ll/position/gps", 1); + ros::ServiceServer mow_service = n.advertiseService("ll/mower/mow_enabled", setMowEnabled); + ros::ServiceServer ros_emergency_service = n.advertiseService("ll/emergency", setEmergencyStop); + ros::Subscriber cmd_vel_sub = n.subscribe("ll/cmd_vel", 0, velReceived, ros::TransportHints().tcpNoDelay(true)); + // ros::Subscriber high_level_status_sub = n.subscribe("/mower_logic/current_state", 0, highLevelStatusReceived); + ros::Timer publish_timer = n.createTimer(ros::Duration(0.5), sendEmergencyHeartbeatTimerTask); + + ctx = xbot::serviceif::Start(true, bind_ip); + + emergency_service = std::make_unique(1, ctx, emergency_pub); + diff_drive_service = std::make_unique( + 2, ctx, actual_twist_pub, status_left_esc_pub, status_right_esc_pub, wheel_ticks_per_m, wheel_distance_m); + mower_service = std::make_unique(3, ctx, status_pub); + imu_service = std::make_unique(4, ctx, sensor_imu_pub); + power_service = std::make_unique(5, ctx, power_pub); + gps_position_service = std::make_unique(6, ctx, gps_position_pub); + lidar_service = std::make_unique(42, ctx, sensor_lidar_pub); + docking_sensor_service = std::make_unique(43, ctx, sensor_dock_pub); + + emergency_service->Start(); + diff_drive_service->Start(); + mower_service->Start(); + imu_service->Start(); + lidar_service->Start(); + power_service->Start(); + docking_sensor_service->Start(); + gps_position_service->Start(); + + ros::spin(); + + return 0; +} diff --git a/src/mower_logic/CMakeLists.txt b/src/mower_logic/CMakeLists.txt index ab2d1316..c9065460 100644 --- a/src/mower_logic/CMakeLists.txt +++ b/src/mower_logic/CMakeLists.txt @@ -8,22 +8,22 @@ project(mower_logic) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS - dynamic_reconfigure - ftc_local_planner - mbf_msgs - mower_map - mower_msgs - nav_msgs - rosbag - roscpp - slic3r_coverage_planner - tf2 - tf2_geometry_msgs - tf2_ros - sensor_msgs - robot_localization - xbot_msgs - xbot_positioning + dynamic_reconfigure + ftc_local_planner + mbf_msgs + mower_map + mower_msgs + nav_msgs + rosbag + roscpp + slic3r_coverage_planner + tf2 + tf2_geometry_msgs + tf2_ros + sensor_msgs + robot_localization + xbot_msgs + xbot_positioning ) find_package(PkgConfig REQUIRED) @@ -64,8 +64,8 @@ pkg_check_modules(CRYPTOPP REQUIRED libcrypto++) ## Generate messages in the 'msg' folder add_message_files( - FILES - CheckPoint.msg + FILES + CheckPoint.msg ) ## Generate services in the 'srv' folder @@ -84,8 +84,8 @@ add_message_files( ## Generate added messages and services with any dependencies listed here generate_messages( -# DEPENDENCIES -# mbf_msgs# mower_msgs# nav_msgs# tf2_geometry_msgs + # DEPENDENCIES + # mbf_msgs# mower_msgs# nav_msgs# tf2_geometry_msgs ) ################################################ @@ -103,10 +103,10 @@ generate_messages( ## and list every .cfg file to be processed ## Generate dynamic reconfigure parameters in the 'cfg' folder - generate_dynamic_reconfigure_options( - cfg/MowerLogic.cfg - cfg/MowerOdometry.cfg - ) +generate_dynamic_reconfigure_options( + cfg/MowerLogic.cfg + cfg/MowerOdometry.cfg +) ################################### ## catkin specific configuration ## @@ -118,10 +118,10 @@ generate_messages( ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( -# INCLUDE_DIRS include -# LIBRARIES mower_logic -# CATKIN_DEPENDS dynamic_reconfigure ftc_local_planner mbf_msgs mower_map mower_msgs nav_msgs roscpp slic3r_coverage_planner tf2 tf2_geometry_msgs tf2_ros -# DEPENDS system_lib + # INCLUDE_DIRS include + # LIBRARIES mower_logic + # CATKIN_DEPENDS dynamic_reconfigure ftc_local_planner mbf_msgs mower_map mower_msgs nav_msgs roscpp slic3r_coverage_planner tf2 tf2_geometry_msgs tf2_ros + # DEPENDS system_lib ) ########### @@ -131,9 +131,9 @@ catkin_package( ## Specify additional locations of header files ## Your package locations should be listed before other locations include_directories( -# include - ${catkin_INCLUDE_DIRS} - ${CRYPTOPP_INCLUDE_DIRS} + # include + ${catkin_INCLUDE_DIRS} + ${CRYPTOPP_INCLUDE_DIRS} ) ## Declare a C++ library @@ -181,18 +181,10 @@ add_executable(mower_logic src/mower_logic/behaviors/PerimeterDocking.cpp src/mower_logic/behaviors/AreaRecordingBehavior.h src/mower_logic/behaviors/AreaRecordingBehavior.cpp - ) +) add_dependencies(mower_logic ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(mower_logic ${catkin_LIBRARIES} ${CRYPTOPP_LIBRARIES}) -add_executable(mower_odometry src/mower_odometry/mower_odometry.cpp) -add_dependencies(mower_odometry ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) -target_link_libraries(mower_odometry ${catkin_LIBRARIES}) - -add_executable(mag_calibration src/mag_calibration/mag_calibration.cpp) -add_dependencies(mag_calibration ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) -target_link_libraries(mag_calibration ${catkin_LIBRARIES}) - add_executable(monitoring src/monitoring/monitoring.cpp) add_dependencies(monitoring ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(monitoring ${catkin_LIBRARIES}) diff --git a/src/mower_logic/src/mag_calibration/mag_calibration.cpp b/src/mower_logic/src/mag_calibration/mag_calibration.cpp deleted file mode 100644 index 6a916a2a..00000000 --- a/src/mower_logic/src/mag_calibration/mag_calibration.cpp +++ /dev/null @@ -1,90 +0,0 @@ -// Created by Clemens Elflein on 3/28/22. -// Copyright (c) 2022 Clemens Elflein. All rights reserved. -// -// This work is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License. -// -// Feel free to use the design in your private/educational projects, but don't try to sell the design or products based -// on it without getting my consent first. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -// -// - -#include - -#include "ros/ros.h" - -double minX, minY, minZ, maxX, maxY, maxZ; - -double filteredX, filteredY, filteredZ; -uint valueCount = 0; - -double filterFactor = 0.9; - -void magReceived(const sensor_msgs::MagneticField::ConstPtr &msg) { - if (valueCount == 0) { - filteredX = msg->magnetic_field.x; - filteredY = msg->magnetic_field.y; - filteredZ = msg->magnetic_field.z; - valueCount++; - return; - } - - valueCount++; - - filteredX = filterFactor * filteredX + (1.0 - filterFactor) * msg->magnetic_field.x; - filteredY = filterFactor * filteredY + (1.0 - filterFactor) * msg->magnetic_field.y; - filteredZ = filterFactor * filteredZ + (1.0 - filterFactor) * msg->magnetic_field.z; - - if (valueCount < 100) { - return; - } - - minX = std::min(minX, filteredX); - minY = std::min(minY, filteredY); - minZ = std::min(minZ, filteredZ); - maxX = std::max(maxX, filteredX); - maxY = std::max(maxY, filteredY); - maxZ = std::max(maxZ, filteredZ); - - ROS_INFO_STREAM_THROTTLE( - 1, "\n" - << "min_x = " << minX << "; max_x = " << maxX << "; x_bias = " << (minX + maxX) / 2.0 << "\n" - << "min_y = " << minY << "; max_y = " << maxY << "; y_bias = " << (minY + maxY) / 2.0 << "\n" - << "min_z = " << minZ << "; max_z = " << maxZ << "; z_bias = " << (minZ + maxZ) / 2.0 << "\n"); -} - -int main(int argc, char **argv) { - ros::init(argc, argv, "mag_calibration"); - - minX = minY = minZ = INFINITY; - maxX = maxY = maxZ = -INFINITY; - - std::cout << "Rotate the mower around all axes and press enter once you are done."; - sleep(2); - - ros::NodeHandle n; - ros::Subscriber mag_sub = n.subscribe("imu/mag", 100, magReceived); - - ros::AsyncSpinner spinner(1); - spinner.start(); - - while (ros::ok()) { - std::string line; - std::getline(std::cin, line); - break; - } - - std::cout << "Done, use the following lines in your config:\n" - << "export OM_MAG_BIAS_X=" << (minX + maxX) / 2.0 << "\n" - << "export OM_MAG_BIAS_Y=" << (minY + maxY) / 2.0 << "\n" - << "export OM_MAG_BIAS_Z=" << (minZ + maxZ) / 2.0 << "\n"; - - return 0; -} diff --git a/src/mower_logic/src/monitoring/monitoring.cpp b/src/mower_logic/src/monitoring/monitoring.cpp index 1ddb2d50..6879ade5 100644 --- a/src/mower_logic/src/monitoring/monitoring.cpp +++ b/src/mower_logic/src/monitoring/monitoring.cpp @@ -17,6 +17,9 @@ // #include +#include +#include +#include #include "mower_logic/MowerLogicConfig.h" #include "mower_msgs/HighLevelStatus.h" @@ -32,21 +35,20 @@ xbot_msgs::RobotState state; ros::NodeHandle *n; -ros::Time last_status_update(0); -ros::Time last_pose_update(0); - ros::NodeHandle *paramNh; dynamic_reconfigure::Client *reconfigClient; mower_logic::MowerLogicConfig mower_logic_config; +bool mower_logic_config_valid = false; typedef const mower_msgs::Status::ConstPtr StatusPtr; // Sensor configuration struct SensorConfig { - std::string name; // Speaking name, used in sensor widget - std::string unit; // Unit like A, V, ... - uint8_t value_desc; + std::string name; // Speaking name, used in sensor widget + std::string unit; // Unit like A, V, ... + uint8_t value_desc; // Voltage, Current, RPM, ... + uint8_t sensor_type; // Double, String, ... std::function getStatusSensorValueCB = nullptr; std::function setSensorLimitsCB = nullptr; std::string param_path = ""; // Path to parameters @@ -68,23 +70,25 @@ void set_limits_mow_motor_temp(SensorConfig &sensor_config); // Place all sensors in a key=sensor.id -> SensorConfig map // clang-format off std::map sensor_configs{ - {"om_v_charge", {"V Charge", "V", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_VOLTAGE, [](StatusPtr msg) { return msg->v_charge; }, &set_limits_charge_v}}, - {"om_v_battery", {"V Battery", "V", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_VOLTAGE, [](StatusPtr msg) { return msg->v_battery; }, &set_limits_battery_v}}, - {"om_charge_current", {"Charge Current", "A", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_CURRENT, [](StatusPtr msg) { return msg->charge_current; }, &set_limits_charge_current, "", [](){ return !paramNh->param("/mower_logic/ignore_charging_current", false); }}}, - {"om_left_esc_temp", {"Left ESC Temp", "deg.C", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_TEMPERATURE, [](StatusPtr msg) { return msg->left_esc_status.temperature_pcb; }, &set_limits_esc_temp, "left_xesc"}}, - {"om_right_esc_temp", {"Right ESC Temp", "deg.C", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_TEMPERATURE, [](StatusPtr msg) { return msg->right_esc_status.temperature_pcb; }, &set_limits_esc_temp, "right_xesc"}}, - {"om_mow_esc_temp", {"Mow ESC Temp", "deg.C", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_TEMPERATURE, [](StatusPtr msg) { return msg->mow_esc_status.temperature_pcb; }, &set_limits_esc_temp, "mower_xesc"}}, - {"om_mow_motor_temp", {"Mow Motor Temp", "deg.C", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_TEMPERATURE, [](StatusPtr msg) { return msg->mow_esc_status.temperature_motor; }, &set_limits_mow_motor_temp, "mower_xesc", [](){ return paramNh->param("mower_xesc/has_motor_temp", true); }}}, - {"om_mow_motor_current", {"Mow Motor Current", "A", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_CURRENT, [](StatusPtr msg) { return msg->mow_esc_status.current; }, &set_limits_mow_motor_current, "mower_xesc"}}, - {"om_mow_motor_rpm", {"Mow Motor Revolutions", "rpm", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_RPM, [](StatusPtr msg) { return msg->mow_esc_status.rpm; }, &set_limits_mow_motor_rpm, "mower_xesc"}}, - {"om_gps_accuracy", {"GPS Accuracy", "m", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_DISTANCE}}, + {"om_v_charge", {"V Charge", "V", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_VOLTAGE, xbot_msgs::SensorInfo::TYPE_DOUBLE, nullptr, &set_limits_charge_v}}, + {"om_v_battery", {"V Battery", "V", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_VOLTAGE, xbot_msgs::SensorInfo::TYPE_DOUBLE, nullptr, &set_limits_battery_v}}, + {"om_charge_current", {"Charge Current", "A", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_CURRENT, xbot_msgs::SensorInfo::TYPE_DOUBLE, nullptr, &set_limits_charge_current, "", [](){ return !paramNh->param("/mower_logic/ignore_charging_current", false); }}}, + {"om_charge_state", {"Charge State", "", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_UNKNOWN, xbot_msgs::SensorInfo::TYPE_STRING, nullptr}}, + {"om_left_esc_temp", {"Left ESC Temp", "deg.C", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_TEMPERATURE, xbot_msgs::SensorInfo::TYPE_DOUBLE, nullptr, &set_limits_esc_temp, "left_xesc"}}, + {"om_right_esc_temp", {"Right ESC Temp", "deg.C", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_TEMPERATURE, xbot_msgs::SensorInfo::TYPE_DOUBLE, nullptr, &set_limits_esc_temp, "right_xesc"}}, + {"om_mow_esc_temp", {"Mow ESC Temp", "deg.C", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_TEMPERATURE, xbot_msgs::SensorInfo::TYPE_DOUBLE, [](StatusPtr msg) { return msg->mower_esc_temperature; }, &set_limits_esc_temp, "mower_xesc"}}, + {"om_mow_motor_temp", {"Mow Motor Temp", "deg.C", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_TEMPERATURE, xbot_msgs::SensorInfo::TYPE_DOUBLE, [](StatusPtr msg) { return msg->mower_motor_temperature; }, &set_limits_mow_motor_temp, "mower_xesc", [](){ return paramNh->param("mower_xesc/has_motor_temp", true); }}}, + {"om_mow_motor_current", {"Mow Motor Current", "A", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_CURRENT, xbot_msgs::SensorInfo::TYPE_DOUBLE, [](StatusPtr msg) { return msg->mower_esc_current; }, &set_limits_mow_motor_current, "mower_xesc"}}, + {"om_mow_motor_rpm", {"Mow Motor Revolutions", "rpm", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_RPM, xbot_msgs::SensorInfo::TYPE_DOUBLE, [](StatusPtr msg) { return msg->mower_motor_rpm; }, &set_limits_mow_motor_rpm, "mower_xesc"}}, + {"om_gps_accuracy", {"GPS Accuracy", "m", xbot_msgs::SensorInfo::VALUE_DESCRIPTION_DISTANCE, xbot_msgs::SensorInfo::TYPE_DOUBLE}}, }; // clang-format on void status(StatusPtr &msg) { // Rate limit to 2Hz - if ((msg->stamp - last_status_update).toSec() < 0.5) return; - last_status_update = msg->stamp; + static ros::Time last_update{0}; + if ((msg->stamp - last_update).toSec() < 0.5) return; + last_update = msg->stamp; xbot_msgs::SensorDataDouble sensor_data; sensor_data.stamp = msg->stamp; @@ -118,8 +122,9 @@ void pose_received(const xbot_msgs::AbsolutePose::ConstPtr &msg) { state.robot_pose = *msg; // Rate limit to 2Hz - if ((msg->header.stamp - last_pose_update).toSec() < 0.5) return; - last_pose_update = msg->header.stamp; + static ros::Time last_update{0}; + if ((msg->header.stamp - last_update).toSec() < 0.5) return; + last_update = msg->header.stamp; xbot_msgs::SensorDataDouble sensor_data; sensor_data.stamp = msg->header.stamp; @@ -130,6 +135,53 @@ void pose_received(const xbot_msgs::AbsolutePose::ConstPtr &msg) { sc_it->second.data_pub.publish(sensor_data); } } +void power_received(const mower_msgs::Power::ConstPtr &msg) { + // Rate limit to 2Hz + static ros::Time last_update{0}; + if ((msg->stamp - last_update).toSec() < 0.5) return; + last_update = msg->stamp; + + { + xbot_msgs::SensorDataDouble sensor_data; + sensor_data.stamp = msg->stamp; + sensor_data.data = msg->v_charge; + + auto sc_it = sensor_configs.find("om_v_charge"); + if (sc_it != std::end(sensor_configs)) { + sc_it->second.data_pub.publish(sensor_data); + } + } + { + xbot_msgs::SensorDataDouble sensor_data; + sensor_data.stamp = msg->stamp; + sensor_data.data = msg->v_battery; + + auto sc_it = sensor_configs.find("om_v_battery"); + if (sc_it != std::end(sensor_configs)) { + sc_it->second.data_pub.publish(sensor_data); + } + } + { + xbot_msgs::SensorDataDouble sensor_data; + sensor_data.stamp = msg->stamp; + sensor_data.data = msg->charge_current; + + auto sc_it = sensor_configs.find("om_charge_current"); + if (sc_it != std::end(sensor_configs)) { + sc_it->second.data_pub.publish(sensor_data); + } + } + { + xbot_msgs::SensorDataString sensor_data; + sensor_data.stamp = msg->stamp; + sensor_data.data = msg->charger_status; + + auto sc_it = sensor_configs.find("om_charge_state"); + if (sc_it != std::end(sensor_configs)) { + sc_it->second.data_pub.publish(sensor_data); + } + } +} void set_limits_battery_v(SensorConfig &sensor_config) { sensor_config.si.lower_critical_value = mower_logic_config.battery_critical_voltage; @@ -164,9 +216,11 @@ void set_limits_mow_motor_rpm(SensorConfig &sensor_config) { void set_limits_mow_motor_temp(SensorConfig &sensor_config) { // mower_config settings have precedence before xesc param file because user editable sensor_config.si.max_value = - (mower_logic_config.motor_hot_temperature ?: paramNh->param(sensor_config.param_path + "/max_motor_temp", 0.0f)); + (mower_logic_config_valid ? mower_logic_config.motor_hot_temperature + : paramNh->param(sensor_config.param_path + "/max_motor_temp", 0.0f)); sensor_config.si.min_value = - (mower_logic_config.motor_cold_temperature ?: paramNh->param(sensor_config.param_path + "/min_motor_temp", 0.0f)); + (mower_logic_config_valid ? mower_logic_config.motor_cold_temperature + : paramNh->param(sensor_config.param_path + "/min_motor_temp", 0.0f)); } void registerSensors() { @@ -210,6 +264,7 @@ void registerSensors() { void reconfigCB(const mower_logic::MowerLogicConfig &config) { ROS_INFO_STREAM("Monitoring received new mower_logic config"); mower_logic_config = config; + mower_logic_config_valid = true; registerSensors(); } diff --git a/src/mower_logic/src/mower_logic/StateSubscriber.h b/src/mower_logic/src/mower_logic/StateSubscriber.h new file mode 100644 index 00000000..6917de56 --- /dev/null +++ b/src/mower_logic/src/mower_logic/StateSubscriber.h @@ -0,0 +1,70 @@ +// +// Created by clemens on 29.11.24. +// + +#ifndef STATESUBSCRIBER_H +#define STATESUBSCRIBER_H + +#include "ros/ros.h" + +template +class StateSubscriber { +public: + explicit StateSubscriber(const std::string &topic); + + void Start(ros::NodeHandle *n); + + MESSAGE getMessage(); + + bool hasMessage(); + + ros::Time getMessageTime(); + + void setMessage(const MESSAGE &message); + +private: + std::string topic_; + std::mutex message_mutex_{}; + MESSAGE message_{}; + ros::Time last_message_time_{}; + bool has_message_ = false; + ros::Subscriber subscriber_{}; +}; + +template +StateSubscriber::StateSubscriber(const std::string &topic) : topic_{topic} { +} + +template +void StateSubscriber::Start(ros::NodeHandle *n) { + n->subscribe(topic_, 10, &StateSubscriber::setMessage, this); +} + +template +MESSAGE StateSubscriber::getMessage() { + std::lock_guard lk{message_mutex_}; + return message_; +} + +template +bool StateSubscriber::hasMessage() { + std::lock_guard lk{message_mutex_}; + return has_message_; +} + +template +ros::Time StateSubscriber::getMessageTime() { + std::lock_guard lk{message_mutex_}; + return last_message_time_; +} + +template +void StateSubscriber::setMessage(const MESSAGE &message) { + std::lock_guard lk{message_mutex_}; + last_message_time_ = ros::Time::now(); + message_ = message; + has_message_ = true; +} + + +#endif //STATESUBSCRIBER_H diff --git a/src/mower_logic/src/mower_logic/behaviors/DockingBehavior.cpp b/src/mower_logic/src/mower_logic/behaviors/DockingBehavior.cpp index 8fd745f1..3547c64b 100644 --- a/src/mower_logic/src/mower_logic/behaviors/DockingBehavior.cpp +++ b/src/mower_logic/src/mower_logic/behaviors/DockingBehavior.cpp @@ -17,12 +17,15 @@ // #include "DockingBehavior.h" +#include + #include "PerimeterDocking.h" extern ros::ServiceClient dockingPointClient; extern actionlib::SimpleActionClient *mbfClient; extern actionlib::SimpleActionClient *mbfClientExePath; extern mower_msgs::Status getStatus(); +extern mower_msgs::Power getPower(); extern void stopMoving(); extern bool setGPS(bool enabled); @@ -119,6 +122,7 @@ bool DockingBehavior::dock_straight() { r.sleep(); const auto last_status = getStatus(); + const auto last_power = getPower(); auto mbfState = mbfClientExePath->getState(); if (aborted) { @@ -133,8 +137,8 @@ bool DockingBehavior::dock_straight() { case actionlib::SimpleClientGoalState::ACTIVE: case actionlib::SimpleClientGoalState::PENDING: // currently moving. Cancel as soon as we're in the station - if (last_status.v_charge > 5.0) { - ROS_INFO_STREAM("Got a voltage of " << last_status.v_charge << " V. Cancelling docking."); + if (last_power.v_charge > 5.0) { + ROS_INFO_STREAM("Got a voltage of " << last_power.v_charge << " V. Cancelling docking."); ros::Duration(config.docking_extra_time).sleep(); mbfClientExePath->cancelGoal(); stopMoving(); @@ -144,8 +148,8 @@ bool DockingBehavior::dock_straight() { break; case actionlib::SimpleClientGoalState::SUCCEEDED: // we stopped moving because the path has ended. check, if we have docked successfully - ROS_INFO_STREAM("Docking stopped, because we reached end pose. Voltage was " << last_status.v_charge << " V."); - if (last_status.v_charge > 5.0) { + ROS_INFO_STREAM("Docking stopped, because we reached end pose. Voltage was " << last_power.v_charge << " V."); + if (last_power.v_charge > 5.0) { mbfClientExePath->cancelGoal(); dockingSuccess = true; stopMoving(); @@ -172,7 +176,7 @@ std::string DockingBehavior::state_name() { Behavior *DockingBehavior::execute() { // Check if already docked (e.g. carried to base during emergency) and skip - if (getStatus().v_charge > 5.0) { + if (getPower().v_charge > 5.0) { ROS_INFO_STREAM("Already inside docking station, going directly to idle."); stopMoving(); return &IdleBehavior::DOCKED_INSTANCE; diff --git a/src/mower_logic/src/mower_logic/behaviors/IdleBehavior.cpp b/src/mower_logic/src/mower_logic/behaviors/IdleBehavior.cpp index 15ce2c87..eca76fa5 100644 --- a/src/mower_logic/src/mower_logic/behaviors/IdleBehavior.cpp +++ b/src/mower_logic/src/mower_logic/behaviors/IdleBehavior.cpp @@ -17,6 +17,8 @@ // #include "IdleBehavior.h" +#include + #include "PerimeterDocking.h" extern void stopMoving(); @@ -29,6 +31,7 @@ extern ros::Time rain_resume; extern ros::ServiceClient dockingPointClient; extern mower_msgs::Status getStatus(); +extern mower_msgs::Power getPower(); extern mower_logic::MowerLogicConfig getConfig(); extern dynamic_reconfigure::Server *reconfigServer; @@ -70,6 +73,7 @@ Behavior *IdleBehavior::execute() { stopBlade(); const auto last_config = getConfig(); const auto last_status = getStatus(); + const auto last_power = getPower(); const bool automatic_mode = last_config.automatic_mode == eAutoMode::AUTO; const bool active_semiautomatic_task = last_config.automatic_mode == eAutoMode::SEMIAUTO && @@ -79,13 +83,13 @@ Behavior *IdleBehavior::execute() { if (rain_delay) { ROS_INFO_STREAM_THROTTLE(300, "Rain delay: " << int((rain_resume - ros::Time::now()).toSec() / 60) << " minutes"); } - const bool mower_ready = last_status.v_battery > last_config.battery_full_voltage && - last_status.mow_esc_status.temperature_motor < last_config.motor_cold_temperature && + const bool mower_ready = last_power.v_battery > last_config.battery_full_voltage && + last_status.mower_motor_temperature < last_config.motor_cold_temperature && !last_config.manual_pause_mowing && !rain_delay; if (manual_start_mowing || ((automatic_mode || active_semiautomatic_task) && mower_ready)) { // set the robot's position to the dock if we're actually docked - if (last_status.v_charge > 5.0) { + if (last_power.v_charge > 5.0) { if (PerimeterUndockingBehavior::configured(config)) return &PerimeterUndockingBehavior::INSTANCE; ROS_INFO_STREAM("Currently inside the docking station, we set the robot's pose to the docks pose."); setRobotPose(docking_pose_stamped.pose); @@ -105,7 +109,7 @@ Behavior *IdleBehavior::execute() { return &IdleBehavior::INSTANCE; } - if (last_config.docking_redock && stay_docked && last_status.v_charge < 5.0) { + if (last_config.docking_redock && stay_docked && last_power.v_charge < 5.0) { ROS_WARN("We docked but seem to have lost contact with the charger. Undocking and trying again!"); return &UndockingBehavior::RETRY_INSTANCE; } diff --git a/src/mower_logic/src/mower_logic/behaviors/PerimeterDocking.cpp b/src/mower_logic/src/mower_logic/behaviors/PerimeterDocking.cpp index ab111d9d..ff9eef17 100644 --- a/src/mower_logic/src/mower_logic/behaviors/PerimeterDocking.cpp +++ b/src/mower_logic/src/mower_logic/behaviors/PerimeterDocking.cpp @@ -1,5 +1,7 @@ #include "PerimeterDocking.h" +#include + #include "IdleBehavior.h" #include "mower_msgs/Perimeter.h" #include "mower_msgs/PerimeterControlSrv.h" @@ -20,6 +22,7 @@ extern ros::NodeHandle* n; extern ros::Publisher cmd_vel_pub; extern mower_msgs::Status getStatus(); +extern mower_msgs::Power getPower(); extern void setGPS(bool enabled); static ros::Subscriber perimeterSubscriber; @@ -225,7 +228,7 @@ Behavior* PerimeterDockingBehavior::arrived() { ROS_WARN("Travelled %.f meters before reaching the station", travelled); return &IdleBehavior::INSTANCE; } - if (getStatus().v_charge > 5.0) { + if (getPower().v_charge > 5.0) { chargeSeen++; if (chargeSeen >= 2) { chargeSeen = 0; diff --git a/src/mower_logic/src/mower_logic/behaviors/UndockingBehavior.cpp b/src/mower_logic/src/mower_logic/behaviors/UndockingBehavior.cpp index 1c5eb604..c90f2457 100644 --- a/src/mower_logic/src/mower_logic/behaviors/UndockingBehavior.cpp +++ b/src/mower_logic/src/mower_logic/behaviors/UndockingBehavior.cpp @@ -17,12 +17,15 @@ // #include "UndockingBehavior.h" +#include + #include "tf2_eigen/tf2_eigen.h" extern ros::ServiceClient dockingPointClient; extern actionlib::SimpleActionClient *mbfClientExePath; extern xbot_msgs::AbsolutePose getPose(); extern mower_msgs::Status getStatus(); +extern mower_msgs::Power getPower(); extern void setRobotPose(geometry_msgs::Pose &pose); extern void stopMoving(); @@ -136,7 +139,7 @@ void UndockingBehavior::enter() { docking_pose_stamped.header.stamp = ros::Time::now(); // set the robot's position to the dock if we're actually docked - if (getStatus().v_charge > 5.0) { + if (getPower().v_charge > 5.0) { ROS_INFO_STREAM("Currently inside the docking station, we set the robot's pose to the docks pose."); setRobotPose(docking_pose_stamped.pose); } diff --git a/src/mower_logic/src/mower_logic/mower_logic.cpp b/src/mower_logic/src/mower_logic/mower_logic.cpp index c3bf169d..e36683b7 100644 --- a/src/mower_logic/src/mower_logic/mower_logic.cpp +++ b/src/mower_logic/src/mower_logic/mower_logic.cpp @@ -20,6 +20,9 @@ #include #include +#include +#include +#include #include #include @@ -27,7 +30,7 @@ #include #include -#include "actionlib/client/simple_client_goal_state.h" +#include "StateSubscriber.h" #include "behaviors/AreaRecordingBehavior.h" #include "behaviors/Behavior.h" #include "behaviors/IdleBehavior.h" @@ -39,19 +42,15 @@ #include "mower_map/ClearNavPointSrv.h" #include "mower_map/GetDockingPointSrv.h" #include "mower_map/GetMowingAreaSrv.h" -#include "mower_map/SetDockingPointSrv.h" #include "mower_map/SetNavPointSrv.h" #include "mower_msgs/EmergencyStopSrv.h" #include "mower_msgs/HighLevelControlSrv.h" #include "mower_msgs/HighLevelStatus.h" #include "mower_msgs/MowerControlSrv.h" #include "mower_msgs/Status.h" -#include "nav_msgs/Odometry.h" -#include "nav_msgs/Path.h" #include "ros/ros.h" #include "slic3r_coverage_planner/PlanPath.h" #include "std_msgs/String.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" #include "xbot_msgs/AbsolutePose.h" #include "xbot_msgs/RegisterActionsSrv.h" #include "xbot_positioning/GPSControlSrv.h" @@ -70,11 +69,12 @@ actionlib::SimpleActionClient *mbfClientExePath; ros::Publisher cmd_vel_pub, high_level_state_publisher; mower_logic::MowerLogicConfig last_config; -// store some values for safety checks -ros::Time pose_time(0.0); -xbot_msgs::AbsolutePose last_pose; -ros::Time status_time(0.0); -mower_msgs::Status last_status; +StateSubscriber emergency_state_subscriber{"/ll/emergency"}; +StateSubscriber status_state_subscriber{"/ll/mower_status"}; +StateSubscriber power_state_subscriber{"/ll/power"}; +StateSubscriber left_esc_status_state_subscriber{"/ll/diff_drive/left_esc_status"}; +StateSubscriber right_esc_status_state_subscriber{"/ll/diff_drive/right_esc_status"}; +StateSubscriber pose_state_subscriber{"/xbot_positioning/xb_pose"}; ros::Time last_good_gps(0.0); @@ -97,16 +97,6 @@ ros::Time rain_resume; /** * Some thread safe methods to get a copy of the logic state */ -ros::Time getPoseTime() { - std::lock_guard lk{mower_logic_mutex}; - return pose_time; -} - -ros::Time getStatusTime() { - std::lock_guard lk{mower_logic_mutex}; - return status_time; -} - ros::Time getLastGoodGPS() { std::lock_guard lk{mower_logic_mutex}; return last_good_gps; @@ -117,11 +107,6 @@ void setLastGoodGPS(ros::Time time) { last_good_gps = time; } -mower_msgs::Status getStatus() { - std::lock_guard lk{mower_logic_mutex}; - return last_status; -} - mower_logic::MowerLogicConfig getConfig() { std::lock_guard lk{mower_logic_mutex}; return last_config; @@ -133,9 +118,14 @@ void setConfig(mower_logic::MowerLogicConfig c) { reconfigServer->updateConfig(c); } +mower_msgs::Status getStatus() { + return status_state_subscriber.getMessage(); +} +mower_msgs::Power getPower() { + return power_state_subscriber.getMessage(); +} xbot_msgs::AbsolutePose getPose() { - std::lock_guard lk{mower_logic_mutex}; - return last_pose; + return pose_state_subscriber.getMessage(); } void setEmergencyMode(bool emergency); @@ -159,10 +149,9 @@ void registerActions(std::string prefix, const std::vector lk{mower_logic_mutex}; - last_pose.pose.pose = pose; - } + auto last_pose = pose_state_subscriber.getMessage(); + last_pose.pose.pose = pose; + pose_state_subscriber.setMessage(last_pose); xbot_positioning::SetPoseSrv pose_srv; pose_srv.request.robot_pose = pose; @@ -185,27 +174,6 @@ void setRobotPose(geometry_msgs::Pose &pose) { } } -void poseReceived(const xbot_msgs::AbsolutePose::ConstPtr &msg) { - std::lock_guard lk{mower_logic_mutex}; - - last_pose = *msg; - -#ifdef VERBOSE_DEBUG - ROS_INFO("om_mower_logic: pose received with accuracy %f", last_pose.position_accuracy); -#endif - pose_time = ros::Time::now(); -} - -void statusReceived(const mower_msgs::Status::ConstPtr &msg) { - std::lock_guard lk{mower_logic_mutex}; - -#ifdef VERBOSE_DEBUG - ROS_INFO("om_mower_logic: statusReceived"); -#endif - last_status = *msg; - status_time = ros::Time::now(); -} - // Abort the currently running behaviour void abortExecution() { if (currentBehavior != nullptr) { @@ -251,6 +219,7 @@ bool setMowerEnabled(bool enabled) { } // status change ? + const auto last_status = status_state_subscriber.getMessage(); if (last_status.mow_enabled != enabled) { ros::Time started = ros::Time::now(); mower_msgs::MowerControlSrv mow_srv; @@ -392,6 +361,7 @@ bool isGpsGood() { // GPS is good if orientation is valid, we have low accuracy and we have a recent GPS update. // TODO: think about the "recent gps flag" since it only looks at the time. E.g. if we were standing still this would // still pause even if no GPS updates are needed during standstill. + const auto last_pose = pose_state_subscriber.getMessage(); return last_pose.orientation_valid && last_pose.position_accuracy < last_config.max_position_accuracy && (last_pose.flags & xbot_msgs::AbsolutePose::FLAG_SENSOR_FUSION_RECENT_ABSOLUTE_POSE); } @@ -400,26 +370,33 @@ bool isGpsGood() { /// /odom and /mower/status outages /// @param timer_event void checkSafety(const ros::TimerEvent &timer_event) { - const auto last_status = getStatus(); + const auto last_status = status_state_subscriber.getMessage(); + const auto last_emergency = emergency_state_subscriber.getMessage(); const auto last_config = getConfig(); - const auto last_pose = getPose(); - const auto pose_time = getPoseTime(); - const auto status_time = getStatusTime(); + const auto last_pose = pose_state_subscriber.getMessage(); + const auto last_power = power_state_subscriber.getMessage(); + const auto last_left_esc_state = left_esc_status_state_subscriber.getMessage(); + const auto last_left_esc_state_time = left_esc_status_state_subscriber.getMessageTime(); + const auto last_right_esc_state = right_esc_status_state_subscriber.getMessage(); + const auto last_right_esc_state_time = right_esc_status_state_subscriber.getMessageTime(); + const auto pose_time = pose_state_subscriber.getMessageTime(); + const auto status_time = status_state_subscriber.getMessageTime(); + const auto power_time = power_state_subscriber.getMessageTime(); const auto last_good_gps = getLastGoodGPS(); - high_level_status.emergency = last_status.emergency; - high_level_status.is_charging = last_status.v_charge > 10.0; + high_level_status.emergency = last_emergency.latched_emergency; + high_level_status.is_charging = last_power.v_charge > 10.0; // Initialize to true, if after all checks it is still true then mower should be enabled. mowerAllowed = true; // send to idle if emergency and we're not recording if (currentBehavior != nullptr) { - if (last_status.emergency) { + if (last_emergency.latched_emergency) { currentBehavior->requestPause(pauseType::PAUSE_EMERGENCY); if (currentBehavior == &AreaRecordingBehavior::INSTANCE || currentBehavior == &IdleBehavior::INSTANCE || currentBehavior == &IdleBehavior::DOCKED_INSTANCE) { - if (last_status.v_charge > 10.0) { + if (last_power.v_charge > 10.0) { // emergency and docked and idle or area recording, so it's safe to reset the emergency mode, reset it. It's // safe since we won't start moving in this mode. setEmergencyMode(false); @@ -443,7 +420,7 @@ void checkSafety(const ros::TimerEvent &timer_event) { // check if status is current. if not, we have a problem since it contains wheel ticks and so on. // Since these should never drop out, we enter emergency instead of "only" stopping - if (ros::Time::now() - status_time > ros::Duration(3)) { + if (ros::Time::now() - status_time > ros::Duration(3) || ros::Time::now() - power_time > ros::Duration(3)) { setEmergencyMode(true); ROS_WARN_STREAM_THROTTLE( 5, "om_mower_logic: EMERGENCY /mower/status values stopped. dt was: " << (ros::Time::now() - status_time)); @@ -451,11 +428,11 @@ void checkSafety(const ros::TimerEvent &timer_event) { } // If the motor controllers error, we enter emergency mode in the hope to save them. They should not error. - if (last_status.right_esc_status.status <= mower_msgs::ESCStatus::ESC_STATUS_ERROR || - last_status.left_esc_status.status <= mower_msgs::ESCStatus::ESC_STATUS_ERROR) { + if (last_left_esc_state.status <= mower_msgs::ESCStatus::ESC_STATUS_ERROR || + last_right_esc_state.status <= mower_msgs::ESCStatus::ESC_STATUS_ERROR) { setEmergencyMode(true); ROS_ERROR_STREAM("EMERGENCY: at least one motor control errored. errors left: " - << (last_status.left_esc_status) << ", status right: " << last_status.right_esc_status); + << (last_left_esc_state.status) << ", status right: " << last_right_esc_state.status); return; } @@ -497,7 +474,7 @@ void checkSafety(const ros::TimerEvent &timer_event) { // enable the mower (if not aleady) if mowerAllowed is still true after checks and bahavior agrees setMowerEnabled(currentBehavior != nullptr && mowerAllowed && currentBehavior->mower_enabled()); - double battery_percent = (last_status.v_battery - last_config.battery_empty_voltage) / + double battery_percent = (last_power.v_battery - last_config.battery_empty_voltage) / (last_config.battery_full_voltage - last_config.battery_empty_voltage); if (battery_percent > 1.0) { battery_percent = 1.0; @@ -517,13 +494,13 @@ void checkSafety(const ros::TimerEvent &timer_event) { } // Dock if below critical voltage to avoid BMS undervoltage protection - if (!dockingNeeded && (last_status.v_battery < last_config.battery_critical_voltage)) { - dockingReason << "Battery voltage min critical: " << last_status.v_battery; + if (!dockingNeeded && (last_power.v_battery < last_config.battery_critical_voltage)) { + dockingReason << "Battery voltage min critical: " << last_power.v_battery; dockingNeeded = true; } // Otherwise take the max battery voltage over 20s to ignore droop during short current spikes - max_v_battery_seen = std::max(max_v_battery_seen, last_status.v_battery); + max_v_battery_seen = std::max(max_v_battery_seen, last_power.v_battery); if (ros::Time::now() - last_v_battery_check > ros::Duration(20.0)) { if (!dockingNeeded && (max_v_battery_seen < last_config.battery_empty_voltage)) { dockingReason << "Battery average voltage low: " << max_v_battery_seen; @@ -533,8 +510,8 @@ void checkSafety(const ros::TimerEvent &timer_event) { last_v_battery_check = ros::Time::now(); } - if (!dockingNeeded && last_status.mow_esc_status.temperature_motor >= last_config.motor_hot_temperature) { - dockingReason << "Mow motor over temp: " << last_status.mow_esc_status.temperature_motor; + if (!dockingNeeded && last_status.mower_motor_temperature >= last_config.motor_hot_temperature) { + dockingReason << "Mow motor over temp: " << last_status.mower_motor_temperature; dockingNeeded = true; } @@ -665,9 +642,6 @@ int main(int argc, char **argv) { cmd_vel_pub = n->advertise("/logic_vel", 1); - ros::Publisher path_pub; - - path_pub = n->advertise("mower_logic/mowing_path", 100, true); high_level_state_publisher = n->advertise("mower_logic/current_state", 100, true); pathClient = n->serviceClient("slic3r_coverage_planner/plan_path"); @@ -692,9 +666,13 @@ int main(int argc, char **argv) { mbfClient = new actionlib::SimpleActionClient("/move_base_flex/move_base"); mbfClientExePath = new actionlib::SimpleActionClient("/move_base_flex/exe_path"); - ros::Subscriber status_sub = n->subscribe("/mower/status", 0, statusReceived, ros::TransportHints().tcpNoDelay(true)); - ros::Subscriber pose_sub = - n->subscribe("/xbot_positioning/xb_pose", 0, poseReceived, ros::TransportHints().tcpNoDelay(true)); + emergency_state_subscriber.Start(n); + status_state_subscriber.Start(n); + power_state_subscriber.Start(n); + left_esc_status_state_subscriber.Start(n); + right_esc_status_state_subscriber.Start(n); + pose_state_subscriber.Start(n); + ros::Subscriber joy_cmd = n->subscribe("/joy_vel", 0, joyVelReceived, ros::TransportHints().tcpNoDelay(true)); ros::Subscriber action = n->subscribe("xbot/action", 0, actionReceived, ros::TransportHints().tcpNoDelay(true)); @@ -705,8 +683,29 @@ int main(int argc, char **argv) { ros::Rate r(1.0); + ROS_INFO("Waiting for emergency message"); + while (!emergency_state_subscriber.hasMessage()) { + if (!ros::ok()) { + delete (reconfigServer); + delete (mbfClient); + delete (mbfClientExePath); + return 1; + } + r.sleep(); + } + ROS_INFO("Waiting for a power message"); + while (!power_state_subscriber.hasMessage()) { + if (!ros::ok()) { + delete (reconfigServer); + delete (mbfClient); + delete (mbfClientExePath); + return 1; + } + r.sleep(); + } + ROS_INFO("Waiting for a status message"); - while (status_time == ros::Time(0.0)) { + while (!status_state_subscriber.hasMessage()) { if (!ros::ok()) { delete (reconfigServer); delete (mbfClient); @@ -717,7 +716,27 @@ int main(int argc, char **argv) { } ROS_INFO("Waiting for a pose message"); - while (pose_time == ros::Time(0.0)) { + while (!power_state_subscriber.hasMessage()) { + if (!ros::ok()) { + delete (reconfigServer); + delete (mbfClient); + delete (mbfClientExePath); + return 1; + } + r.sleep(); + } + ROS_INFO("Waiting for left ESC status message"); + while (!left_esc_status_state_subscriber.hasMessage()) { + if (!ros::ok()) { + delete (reconfigServer); + delete (mbfClient); + delete (mbfClientExePath); + return 1; + } + r.sleep(); + } + ROS_INFO("Waiting for right ESC status message"); + while (!right_esc_status_state_subscriber.hasMessage()) { if (!ros::ok()) { delete (reconfigServer); delete (mbfClient); @@ -830,7 +849,7 @@ int main(int argc, char **argv) { while ((ros::Time::now() - started).toSec() < 10.0) { ROS_INFO_STREAM("Waiting for an emergency status message"); r.sleep(); - if (last_status.emergency) { + if (emergency_state_subscriber.getMessage().latched_emergency) { ROS_INFO_STREAM("Got emergency, resetting it"); setEmergencyMode(false); break; diff --git a/src/mower_logic/src/mower_odometry/mower_odometry.cpp b/src/mower_logic/src/mower_odometry/mower_odometry.cpp deleted file mode 100644 index ca1d101e..00000000 --- a/src/mower_logic/src/mower_odometry/mower_odometry.cpp +++ /dev/null @@ -1,453 +0,0 @@ -// Created by Clemens Elflein on 3/28/22. -// Copyright (c) 2022 Clemens Elflein. All rights reserved. -// -// This work is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License. -// -// Feel free to use the design in your private/educational projects, but don't try to sell the design or products based -// on it without getting my consent first. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -// SOFTWARE. -// -// - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "mower_logic/MowerOdometryConfig.h" -#include "mower_msgs/GPSControlSrv.h" -#include "robot_localization/navsat_conversions.h" -#include "ros/ros.h" -#include "sensor_msgs/NavSatFix.h" -#include "xbot_msgs/AbsolutePose.h" - -ros::Publisher odometry_pub; -ros::Publisher pose_pub; - -tf2::Vector3 last_gps_pos; -double last_gps_acc_m; -ros::Time last_gps_odometry_time(0.0); -int gps_outlier_count = 0; -int valid_gps_samples = 0; -bool gpsOdometryValid = false; -bool gpsEnabled = true; -// If true, we don't use any sensor fusion just copy and paste the F9R result as odometry -bool use_f9r_sensor_fusion = false; - -sensor_msgs::Imu lastImu; -mower_logic::MowerOdometryConfig config; - -bool hasImuMessage = false; - -// Odometry -bool firstData = true; -mower_msgs::Status last_status; - -// inputs here -double d_wheel_r, d_wheel_l, dt = 1.0; - -// outputs here -double x = 0, y = 0, vx = 0.0, r = 0.0, vy = 0.0, vr = 0.0; -geometry_msgs::Quaternion orientation_result; - -// (ticks / revolution) / (m / revolution) -#define TICKS_PER_M (993.0 / (0.19 * M_PI)) - -tf2_ros::Buffer tfBuffer; - -void publishOdometry() { - static tf2_ros::TransformBroadcaster transform_broadcaster; - - geometry_msgs::TransformStamped odom_trans; - ros::Time current_time = ros::Time::now(); - odom_trans.header.stamp = current_time; - odom_trans.header.frame_id = "map"; - odom_trans.child_frame_id = "base_link"; - - odom_trans.transform.translation.x = x; - odom_trans.transform.translation.y = y; - // TODO: Add logic for 3d odometry - odom_trans.transform.translation.z = 0; - odom_trans.transform.rotation = orientation_result; - - transform_broadcaster.sendTransform(odom_trans); - - // next, we'll publish the odometry message over ROS - nav_msgs::Odometry odom; - odom.header.stamp = current_time; - odom.header.frame_id = "map"; - - // set the position - odom.pose.pose.position.x = x; - odom.pose.pose.position.y = y; - odom.pose.pose.position.z = 0.0; - - odom.pose.pose.orientation = odom_trans.transform.rotation; - - // set the velocity - odom.child_frame_id = "base_link"; - odom.twist.twist.linear.x = vx; - odom.twist.twist.linear.y = vy; - odom.twist.twist.angular.z = vr; - - odom.pose.covariance = {10000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10000.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10000.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 10000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.00001}; - - if (gpsOdometryValid && gpsEnabled && ros::Time::now() - last_gps_odometry_time < ros::Duration(5.0)) { - odom.pose.covariance[0] = last_gps_acc_m * last_gps_acc_m; - odom.pose.covariance[7] = last_gps_acc_m * last_gps_acc_m; - } - - odom.twist.covariance = {0.000001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.000001, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10000.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 10000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.000001}; - - xbot_msgs::AbsolutePose pose; - pose.header = odom.header; - pose.sensor_stamp = 0; - pose.received_stamp = 0; - pose.source = xbot_msgs::AbsolutePose::SOURCE_SENSOR_FUSION; - pose.flags = 0; - pose.orientation_valid = true; - // TODO: send motion vector - pose.motion_vector_valid = false; - // TODO: set real value - pose.position_accuracy = last_gps_acc_m; - // TODO: set real value - pose.orientation_accuracy = 0.01; - pose.pose = odom.pose; - pose.vehicle_heading = r; - pose.motion_heading = r; - - pose_pub.publish(pose); - - // publish the message - odometry_pub.publish(odom); -} - -void imuReceived(const sensor_msgs::Imu::ConstPtr &msg) { - lastImu = *msg; - hasImuMessage = true; -} - -void handleGPSUpdate(tf2::Vector3 gps_pos, double gps_accuracy_m) { - if (config.simulate_gps_outage) { - ROS_INFO_STREAM_THROTTLE(5, "Dropping GPS due to simulated outage!"); - return; - } - - if (gps_accuracy_m > 0.05) { - ROS_INFO_STREAM("dropping gps with accuracy: " << gps_accuracy_m << "m"); - return; - } - - double time_since_last_gps = (ros::Time::now() - last_gps_odometry_time).toSec(); - if (time_since_last_gps > 5.0) { - ROS_WARN_STREAM("Last GPS was " << time_since_last_gps << " seconds ago."); - gpsOdometryValid = false; - valid_gps_samples = 0; - gps_outlier_count = 0; - last_gps_pos = gps_pos; - last_gps_acc_m = gps_accuracy_m; - last_gps_odometry_time = ros::Time::now(); - return; - } - - // ROS_INFO_STREAM("GOT GPS: " << gps_pos.x() << ", " << gps_pos.y()); - - double distance_to_last_gps = (last_gps_pos - gps_pos).length(); - - if (distance_to_last_gps < 5.0) { - // inlier, we treat it normally - - // calculate current base_link position from orientation and distance parameter - - double base_link_x = gps_pos.x() - config.gps_antenna_offset * cos(r); - double base_link_y = gps_pos.y() - config.gps_antenna_offset * sin(r); - - // store the gps as last - last_gps_pos = gps_pos; - last_gps_acc_m = gps_accuracy_m; - last_gps_odometry_time = ros::Time::now(); - - gps_outlier_count = 0; - valid_gps_samples++; - if (!gpsOdometryValid && valid_gps_samples > 10) { - ROS_INFO_STREAM("GPS data now valid"); - ROS_INFO_STREAM("First GPS data, moving odometry to " << base_link_x << ", " << base_link_y); - // we don't even have gps yet, set odometry to first estimate - x = base_link_x; - y = base_link_y; - gpsOdometryValid = true; - } else if (gpsOdometryValid) { - // gps was valid before, we apply the filter - x = x * (1.0 - config.gps_filter_factor) + config.gps_filter_factor * base_link_x; - y = y * (1.0 - config.gps_filter_factor) + config.gps_filter_factor * base_link_y; - } - } else { - ROS_WARN_STREAM("GPS outlier found. Distance was: " << distance_to_last_gps); - gps_outlier_count++; - // ~10 sec - if (gps_outlier_count > 10) { - ROS_ERROR_STREAM("too many outliers, assuming that the current gps value is valid."); - last_gps_pos = gps_pos; - last_gps_acc_m = gps_accuracy_m; - last_gps_odometry_time = ros::Time::now(); - - gpsOdometryValid = false; - valid_gps_samples = 0; - gps_outlier_count = 0; - } - } -} - -void gpsPositionReceived(const xbot_msgs::AbsolutePose::ConstPtr &msg) { - // drop messages if we don't use the GPS (docking / undocking) - if (!gpsEnabled) { - ROS_INFO_STREAM_THROTTLE(1, "Dropped GPS Update, because gpsEnable = false"); - return; - } - - if (msg->source != xbot_msgs::AbsolutePose::SOURCE_GPS) { - ROS_ERROR_STREAM("Please only feed GPS updates here fore now!"); - gpsOdometryValid = false; - return; - } - - if (!(msg->flags & xbot_msgs::AbsolutePose::FLAG_GPS_RTK_FIXED) && - !(msg->flags & xbot_msgs::AbsolutePose::FLAG_GPS_RTK_FLOAT)) { - ROS_INFO_THROTTLE(1, "Dropped GPS update, because it neither has RTK FIXED nor RTK FLOAT"); - gpsOdometryValid = false; - return; - } - - double gps_accuracy_m = msg->position_accuracy; - - tf2::Vector3 gps_pos(msg->pose.pose.position.x, msg->pose.pose.position.y, 0.0); - - handleGPSUpdate(gps_pos, gps_accuracy_m); -} - -void gpsPositionReceivedF9R(const xbot_msgs::AbsolutePose::ConstPtr &msg) { - // drop messages if we don't use the GPS (docking / undocking) - if (!gpsEnabled) { - ROS_INFO_STREAM_THROTTLE(1, "Dropped GPS Update, because gpsEnable = false"); - return; - } - - if (msg->source != xbot_msgs::AbsolutePose::SOURCE_GPS) { - ROS_ERROR_STREAM("Please only feed GPS updates here fore now!"); - gpsOdometryValid = false; - return; - } - - if (!msg->orientation_valid) { - ROS_INFO_THROTTLE(1, "Dropped at least one GPS update due to invalid vehicle heading."); - gpsOdometryValid = false; - return; - } - - if (!(msg->flags & xbot_msgs::AbsolutePose::FLAG_GPS_RTK_FIXED) && - !(msg->flags & xbot_msgs::AbsolutePose::FLAG_GPS_DEAD_RECKONING)) { - ROS_INFO_THROTTLE(1, "Dropped GPS update, because it neither has RTK FIXED nor DR"); - gpsOdometryValid = false; - return; - } - - // Ok, we have a heading and either DR or RTK fix from the F9R. Continue! - - double gps_accuracy_m = msg->position_accuracy; - - // We have "good" GPS according to flags. With dead reckoning, we want to continue driving even if the accuracy says - // otherwise. - if (gps_accuracy_m > 0.25) { - gps_accuracy_m = 0.25; - } - - gpsOdometryValid = true; - last_gps_odometry_time = ros::Time::now(); - - x = msg->pose.pose.position.x; - y = msg->pose.pose.position.y; - vx = msg->motion_vector.x; - vy = msg->motion_vector.y; - r = msg->vehicle_heading; - - r = fmod(r, 2.0 * M_PI); - while (r < 0) { - r += M_PI * 2.0; - } - tf2::Quaternion q_mag(0.0, 0.0, r); - orientation_result = tf2::toMsg(q_mag); - last_gps_acc_m = gps_accuracy_m; -} - -bool statusReceivedOrientation(const mower_msgs::Status::ConstPtr &msg) { - if (!hasImuMessage) { - ROS_INFO_THROTTLE(1, "odometry is waiting for imu message"); - return false; - } - - tf2::Quaternion q; - tf2::fromMsg(lastImu.orientation, q); - - tf2::Matrix3x3 m(q); - double unused1, unused2, yaw; - - m.getRPY(unused1, unused2, yaw); - - yaw += config.imu_offset * (M_PI / 180.0); - yaw = fmod(yaw + (M_PI_2), 2.0 * M_PI); - while (yaw < 0) { - yaw += M_PI * 2.0; - } - - tf2::Quaternion q_mag(0.0, 0.0, yaw); - - r = yaw; - - double d_ticks = (d_wheel_l + d_wheel_r) / 2.0; - - orientation_result = tf2::toMsg(q_mag); - // orientation_result = q_mag; - - x += d_ticks * cos(r); - y += d_ticks * sin(r); - - vy = 0; - vx = d_ticks / dt; - vr = lastImu.angular_velocity.z; - - return true; -} - -bool statusReceivedGyro(const mower_msgs::Status::ConstPtr &msg) { - if (!hasImuMessage) { - ROS_INFO_THROTTLE(1, "odometry is waiting for imu message"); - return false; - } - - // only do odometry if we don't have GPS yet (F9R not calibrated) or we have actively disabled it (docking / - // undocking). - if (gpsEnabled && gpsOdometryValid && use_f9r_sensor_fusion) return true; - - // not sure if -= is the right choice here, but it works for the F9R. The thing is when plotting with the MPU9250 the - // sign of lastImu.angular_velocity.z matches that of the F9R's IMU, so it should be right. - r -= lastImu.angular_velocity.z * dt; - r = fmod(r, 2.0 * M_PI); - while (r < 0) { - r += M_PI * 2.0; - } - - double d_ticks = (d_wheel_l + d_wheel_r) / 2.0; - - tf2::Quaternion q_mag(0.0, 0.0, r); - orientation_result = tf2::toMsg(q_mag); - // orientation_result = q_mag; - - x += d_ticks * cos(r); - y += d_ticks * sin(r); - - vy = 0; - vx = d_ticks / dt; - vr = lastImu.angular_velocity.z; - - return true; -} - -void statusReceived(const mower_msgs::Status::ConstPtr &msg) { - // we need the differences, so initialize in the first run - if (firstData) { - last_status = *msg; - firstData = false; - - return; - } - - dt = (msg->stamp - last_status.stamp).toSec(); - - d_wheel_l = (int32_t)(msg->left_esc_status.tacho - last_status.left_esc_status.tacho) / TICKS_PER_M; - d_wheel_r = -(int32_t)(msg->right_esc_status.tacho - last_status.right_esc_status.tacho) / TICKS_PER_M; - - // ROS_INFO_STREAM("d_wheel_l = " << d_wheel_l << ", d_wheel_r = " << d_wheel_r); - - bool success; - if (!use_f9r_sensor_fusion) { - success = statusReceivedOrientation(msg); - } else { - success = statusReceivedGyro(msg); - } - - last_status = *msg; - - if (success) { - publishOdometry(); - } -} - -void reconfigureCB(mower_logic::MowerOdometryConfig &c, uint32_t level) { - ROS_INFO_STREAM("Setting new Mower Odom Config"); - config = c; -} - -bool setGpsState(mower_msgs::GPSControlSrvRequest &req, mower_msgs::GPSControlSrvResponse &res) { - gpsEnabled = req.gps_enabled; - gpsOdometryValid = false; - ROS_INFO_STREAM("Setting GPS enabled to: " << gpsEnabled); - return true; -} - -int main(int argc, char **argv) { - ros::init(argc, argv, "mower_odometry"); - - ros::NodeHandle n; - ros::NodeHandle paramNh("~"); - - ros::ServiceServer gps_service = n.advertiseService("mower_service/set_gps_state", setGpsState); - - dynamic_reconfigure::Server reconfig_server(paramNh); - reconfig_server.setCallback(reconfigureCB); - - tf2_ros::TransformListener tfListener(tfBuffer); - - odometry_pub = n.advertise("mower/odom", 50); - pose_pub = n.advertise("mower/xb_pose_odom", 50); - - firstData = true; - gps_outlier_count = 0; - gpsOdometryValid = false; - - ros::Subscriber status_sub; - ros::Subscriber imu_sub; - - paramNh.param("use_f9r_sensor_fusion", use_f9r_sensor_fusion, false); - - ros::Subscriber gps_sub; - if (use_f9r_sensor_fusion) { - ROS_INFO("Odometry is using F9R sensor fusion"); - - gps_sub = n.subscribe("xbot_driver_gps/xb_pose", 100, gpsPositionReceivedF9R); - status_sub = n.subscribe("mower/status", 100, statusReceived); - imu_sub = n.subscribe("xbot_driver_gps/imu", 100, imuReceived); - } else { - ROS_INFO("Odometry is using relative positioning."); - gps_sub = n.subscribe("xbot_driver_gps/xb_pose", 100, gpsPositionReceived); - status_sub = n.subscribe("mower/status", 100, statusReceived); - imu_sub = n.subscribe("imu/data", 100, imuReceived); - } - - ros::spin(); - return 0; -} diff --git a/src/mower_msgs/CMakeLists.txt b/src/mower_msgs/CMakeLists.txt index 8a12a708..f495aad2 100644 --- a/src/mower_msgs/CMakeLists.txt +++ b/src/mower_msgs/CMakeLists.txt @@ -13,6 +13,9 @@ add_message_files( ESCStatus.msg HighLevelStatus.msg Perimeter.msg + Emergency.msg + Power.msg + DockingSensor.msg ) add_service_files( @@ -21,7 +24,7 @@ add_service_files( GPSControlSrv.srv MowerControlSrv.srv HighLevelControlSrv.srv - PerimeterControlSrv.srv + PerimeterControlSrv.srv ) ## Generate added messages and services with any dependencies listed here diff --git a/src/mower_msgs/msg/DockingSensor.msg b/src/mower_msgs/msg/DockingSensor.msg new file mode 100644 index 00000000..933c7f92 --- /dev/null +++ b/src/mower_msgs/msg/DockingSensor.msg @@ -0,0 +1,4 @@ + +time stamp +uint8 detected_left +uint8 detected_right diff --git a/src/mower_msgs/msg/Emergency.msg b/src/mower_msgs/msg/Emergency.msg new file mode 100644 index 00000000..115a7978 --- /dev/null +++ b/src/mower_msgs/msg/Emergency.msg @@ -0,0 +1,4 @@ +time stamp +bool active_emergency +bool latched_emergency +string reason diff --git a/src/mower_msgs/msg/Power.msg b/src/mower_msgs/msg/Power.msg new file mode 100644 index 00000000..4caaa195 --- /dev/null +++ b/src/mower_msgs/msg/Power.msg @@ -0,0 +1,7 @@ + +time stamp +float32 v_charge +float32 v_battery +float32 charge_current +bool charger_enabled +string charger_status diff --git a/src/mower_msgs/msg/Status.msg b/src/mower_msgs/msg/Status.msg index eca5b971..954f20c0 100644 --- a/src/mower_msgs/msg/Status.msg +++ b/src/mower_msgs/msg/Status.msg @@ -14,14 +14,11 @@ bool rain_detected bool sound_module_available bool sound_module_busy bool ui_board_available -float32[5] ultrasonic_ranges -bool emergency -float32 v_charge -float32 v_battery -float32 charge_current bool mow_enabled # ESC stuff -ESCStatus left_esc_status -ESCStatus right_esc_status -ESCStatus mow_esc_status +uint8 mower_esc_status +float32 mower_esc_temperature +float32 mower_esc_current +float32 mower_motor_temperature +float32 mower_motor_rpm diff --git a/src/mower_simulation/CMakeLists.txt b/src/mower_simulation/CMakeLists.txt index 6b8a97f0..d7aeb4d5 100644 --- a/src/mower_simulation/CMakeLists.txt +++ b/src/mower_simulation/CMakeLists.txt @@ -157,9 +157,27 @@ include_directories( # ${catkin_LIBRARIES} # ) -add_executable(mower_simulation src/mower_simulation.cpp) +add_executable(mower_simulation + src/mower_simulation.cpp + src/SimRobot.cpp + src/SimRobot.h + src/services/imu_service/imu_service.cpp + src/services/power_service/power_service.cpp + src/services/emergency_service/emergency_service.cpp + src/services/diff_drive_service/diff_drive_service.cpp + src/services/mower_service/mower_service.cpp + src/services/position_service/position_service.cpp +) add_dependencies(mower_simulation ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) -target_link_libraries(mower_simulation ${catkin_LIBRARIES}) +target_link_libraries(mower_simulation PUBLIC ${catkin_LIBRARIES} spdlog::spdlog) + +target_add_service(mower_simulation ImuService ${CMAKE_CURRENT_SOURCE_DIR}/../../services/imu_service.json) +target_add_service(mower_simulation PowerService ${CMAKE_CURRENT_SOURCE_DIR}/../../services/power_service.json) +target_add_service(mower_simulation EmergencyService ${CMAKE_CURRENT_SOURCE_DIR}/../../services/emergency_service.json) +target_add_service(mower_simulation DiffDriveService ${CMAKE_CURRENT_SOURCE_DIR}/../../services/diff_drive_service.json) +target_add_service(mower_simulation MowerService ${CMAKE_CURRENT_SOURCE_DIR}/../../services/mower_service.json) +target_add_service(mower_simulation PositionService ${CMAKE_CURRENT_SOURCE_DIR}/../../services/position_service.json) + ############# ## Install ## diff --git a/src/mower_simulation/launch/_mower_simulation.launch b/src/mower_simulation/launch/_mower_simulation.launch index 6e9e7f32..f698d2fa 100644 --- a/src/mower_simulation/launch/_mower_simulation.launch +++ b/src/mower_simulation/launch/_mower_simulation.launch @@ -1,11 +1,4 @@ - - - - - - - diff --git a/src/mower_simulation/package.xml b/src/mower_simulation/package.xml index acabadb8..90e26226 100644 --- a/src/mower_simulation/package.xml +++ b/src/mower_simulation/package.xml @@ -34,7 +34,6 @@ tf2_ros mower_msgs dynamic_reconfigure - mobile_robot_simulator rqt_reconfigure mower_map rosbridge_server diff --git a/src/mower_simulation/src/SimRobot.cpp b/src/mower_simulation/src/SimRobot.cpp new file mode 100644 index 00000000..d49afcf9 --- /dev/null +++ b/src/mower_simulation/src/SimRobot.cpp @@ -0,0 +1,156 @@ +// +// Created by clemens on 29.11.24. +// + +#include "SimRobot.h" + +#include +#include + +#include + +constexpr double SimRobot::BATTERY_VOLTS_MIN; +constexpr double SimRobot::BATTERY_VOLTS_MAX; +constexpr double SimRobot::CHARGE_CURRENT; +constexpr double SimRobot::CHARGE_VOLTS; + +SimRobot::SimRobot(ros::NodeHandle& nh) : nh_{nh} { +} +void SimRobot::Start() { + std::lock_guard lk{state_mutex_}; + if (started_) { + return; + } + started_ = true; + actual_position_publisher_ = nh_.advertise("actual_position", 50); + timer_ = nh_.createTimer(ros::Duration(0.1), &SimRobot::SimulationStep, this); + timer_.start(); +} + +void SimRobot::GetTwist(double& vx, double& vr) { + std::lock_guard lk{state_mutex_}; + vx = vx_; + vr = vr_; + vx += linear_speed_noise(generator); + vr += angular_speed_noise(generator); +} +void SimRobot::ResetEmergency() { + std::lock_guard lk{state_mutex_}; + emergency_active_ = false; + emergency_latch_ = false; +} +void SimRobot::SetEmergency(bool active, const std::string& reason) { + std::lock_guard lk{state_mutex_}; + emergency_active_ = active; + emergency_latch_ |= active; + emergency_reason_ = reason; +} +void SimRobot::GetEmergencyState(bool& active, bool& latch, std::string& reason) { + std::lock_guard lk{state_mutex_}; + active = emergency_active_; + latch = emergency_latch_; + reason = emergency_reason_; +} +void SimRobot::SetControlTwist(double linear, double angular) { + std::lock_guard lk{state_mutex_}; + vx_ = linear; + vr_ = angular; +} +void SimRobot::GetPosition(double& x, double& y, double& heading) { + std::lock_guard lk{state_mutex_}; + + x = pos_x_; + y = pos_y_; + heading = pos_heading_; + + x += position_noise(generator); + y += position_noise(generator); + heading += heading_noise(generator); + heading = fmod(heading, M_PI * 2.0); + while (heading < 0) { + heading += M_PI * 2.0; + } +} +void SimRobot::GetIsCharging(bool& charging, double& seconds_since_start, std::string& charging_status, + double& charger_volts, double& battery_volts, double& charging_current) { + std::lock_guard lk{state_mutex_}; + charging = is_charging_; + seconds_since_start = (ros::Time::now() - charging_started_time).toSec(); + charging_status = charger_state_; + charger_volts = charger_volts_; + charging_current = charge_current_; + battery_volts = battery_volts_; +} +void SimRobot::SimulationStep(const ros::TimerEvent& te) { + std::lock_guard lk{state_mutex_}; + const auto now = ros::Time::now(); + // Update Position if not in emergency mode + if (!emergency_latch_) { + double time_diff_s = (now - last_update_).toSec(); + double delta_x = (vx_ * cos(pos_heading_)) * time_diff_s; + double delta_y = (vx_ * sin(pos_heading_)) * time_diff_s; + double delta_th = vr_ * time_diff_s; + pos_x_ += delta_x; + pos_y_ += delta_y; + pos_heading_ += delta_th; + pos_heading_ = fmod(pos_heading_, M_PI * 2.0); + if (pos_heading_ < 0) { + pos_heading_ += M_PI * 2.0; + } + } + + // Update Charger Status + if (sqrt((docking_pos_x_ - pos_x_) * (docking_pos_x_ - pos_x_) + + (docking_pos_y_ - pos_y_) * (docking_pos_y_ - pos_y_)) < 0.5) { + if (!is_charging_) { + spdlog::info("Charging"); + is_charging_ = true; + charging_started_time = ros::Time::now(); + } + } else { + if (is_charging_) { + spdlog::info("Stopped Charging"); + is_charging_ = false; + } + } + + if (is_charging_) { + if (battery_volts_ < BATTERY_VOLTS_MAX) { + charger_state_ = "CC"; + battery_volts_ += 0.05; + if (battery_volts_ > BATTERY_VOLTS_MAX) { + battery_volts_ = BATTERY_VOLTS_MAX; + } + charger_volts_ = CHARGE_VOLTS; + charge_current_ = CHARGE_CURRENT; + } else if (charge_current_ > 0.2) { + charger_state_ = "CV"; + battery_volts_ = BATTERY_VOLTS_MAX; + charger_volts_ = CHARGE_VOLTS; + charge_current_ = charge_current_ * 0.99; + } else { + charger_state_ = "Done"; + battery_volts_ = BATTERY_VOLTS_MAX; + charger_volts_ = CHARGE_VOLTS; + charge_current_ = 0; + } + } else { + charger_state_ = "Not Charging"; + battery_volts_ = std::max(BATTERY_VOLTS_MIN, battery_volts_ - 0.001); + charger_volts_ = 0.0; + charge_current_ = 0.0; + } + + last_update_ = now; + + { + actual_position_.header.frame_id = "odom"; + actual_position_.header.seq++; + actual_position_.header.stamp = ros::Time::now(); + actual_position_.pose.pose.position.x = pos_x_; + actual_position_.pose.pose.position.y = pos_y_; + actual_position_publisher_.publish(actual_position_); + } + + spdlog::debug("Position: x:{}, y:{}, heading:{}", pos_x_, pos_y_, pos_heading_); +} diff --git a/src/mower_simulation/src/SimRobot.h b/src/mower_simulation/src/SimRobot.h new file mode 100644 index 00000000..7b954762 --- /dev/null +++ b/src/mower_simulation/src/SimRobot.h @@ -0,0 +1,96 @@ +// +// Created by clemens on 29.11.24. +// + +#ifndef SIMROBOT_H +#define SIMROBOT_H + +#include +#include + +#include +#include +#include + +class SimRobot { +public: + explicit SimRobot(ros::NodeHandle &nh); + void Start(); + + void GetPosition(double &x, double &y); + void GetTwist(double &vx, double &vr); + + void ResetEmergency(); + void SetEmergency(bool active, const std::string &reason); + + void GetEmergencyState(bool &active, bool &latch, std::string &reason); + void SetControlTwist(double linear, double angular); + void GetPosition(double &x, double &y, double &heading); + + void SetDockingPose(double x, double y, double heading); + + void GetIsCharging(bool &charging, double &seconds_since_start, std::string &charging_status, double &charger_volts, double &battery_volts, double &charging_current); + + private: + + // 7 cells + static constexpr double BATTERY_VOLTS_MIN = 3.2*7; + static constexpr double BATTERY_VOLTS_MAX = 4.18*7; + static constexpr double CHARGE_CURRENT = 2.5; + static constexpr double CHARGE_VOLTS = 32.0; + + // Lock for all getters and setters and the simulation step + std::mutex state_mutex_; + + // Position of the simulated dock, used to simulate "docked" and charging voltages + double docking_pos_x_ = 0; + double docking_pos_y_ = 0; + double docking_pos_heading_ = 0; + + bool started_ = false; + // Speed X in m/s + double vx_ = 0; + // Speed rotating in rad/s + double vr_ = 0; + // position and heading. This can be used to simulate GPS + double pos_x_ = 0; + double pos_y_ = 0; + double pos_heading_ = 0; + + // Current Emergency State + bool emergency_active_ = false; + // Latched Emergency + bool emergency_latch_ = false; + std::string emergency_reason_{"Boot"}; + ros::Time last_update_{0}; + ros::NodeHandle nh_; + + bool is_charging_ = false; + ros::Time charging_started_time; + double charger_volts_ = 0; + double battery_volts_ = BATTERY_VOLTS_MIN; + double charge_current_ = 0; + std::string charger_state_{"Unknown"}; + + // Timer for simulation + ros::Timer timer_; + void SimulationStep(const ros::TimerEvent& te); + + + /* + * Generate some noise + */ + std::default_random_engine generator{}; + std::normal_distribution position_noise{0.0, 0.03}; + std::normal_distribution heading_noise{0.0, 0.01}; + std::normal_distribution linear_speed_noise{0.0, 0.02}; + std::normal_distribution angular_speed_noise{0.0, 0.02}; + + + // Debugging + nav_msgs::Odometry actual_position_{}; + ros::Publisher actual_position_publisher_{}; + +}; + +#endif //SIMROBOT_H diff --git a/src/mower_simulation/src/mower_simulation.cpp b/src/mower_simulation/src/mower_simulation.cpp index 864ce151..c32d538f 100644 --- a/src/mower_simulation/src/mower_simulation.cpp +++ b/src/mower_simulation/src/mower_simulation.cpp @@ -18,9 +18,16 @@ #include "ros/ros.h" // Include messages for mower control +#include +#include +#include #include #include +#include +#include + +#include "SimRobot.h" #include "dynamic_reconfigure/server.h" #include "geometry_msgs/PoseWithCovarianceStamped.h" #include "geometry_msgs/Twist.h" @@ -30,6 +37,12 @@ #include "mower_msgs/Status.h" #include "mower_simulation/MowerSimulationConfig.h" #include "nav_msgs/Odometry.h" +#include "services/diff_drive_service/diff_drive_service.hpp" +#include "services/emergency_service/emergency_service.hpp" +#include "services/imu_service/imu_service.hpp" +#include "services/mower_service/mower_service.hpp" +#include "services/position_service/position_service.hpp" +#include "services/power_service/power_service.hpp" #include "xbot_msgs/AbsolutePose.h" #include "xbot_positioning/GPSControlSrv.h" #include "xbot_positioning/SetPoseSrv.h" @@ -40,167 +53,8 @@ ros::Publisher pose_pub; ros::Publisher initial_pose_publisher; ros::ServiceClient docking_point_client; -mower_msgs::Status fake_mow_status; -mower_simulation::MowerSimulationConfig config; dynamic_reconfigure::Server *reconfig_server; -geometry_msgs::Twist last_cmd_vel; - -xbot_msgs::AbsolutePose pose{}; -geometry_msgs::PoseWithCovarianceStamped initialPoseMsg; - -bool has_dock = false; -double dockX = 0, dockY = 0; -bool is_docked = false; - -bool setGpsState(xbot_positioning::GPSControlSrvRequest &req, xbot_positioning::GPSControlSrvResponse &res) { - return true; -} - -bool setMowEnabled(mower_msgs::MowerControlSrvRequest &req, mower_msgs::MowerControlSrvResponse &res) { - config.mower_running = req.mow_enabled; - reconfig_server->updateConfig(config); - return true; -} - -bool setEmergencyStop(mower_msgs::EmergencyStopSrvRequest &req, mower_msgs::EmergencyStopSrvResponse &res) { - config.emergency_stop = req.emergency; - reconfig_server->updateConfig(config); - return true; -} -void fetchDock(const ros::TimerEvent &timer_event) { - mower_map::GetDockingPointSrv get_docking_point_srv; - geometry_msgs::PoseWithCovarianceStamped docking_pose_stamped; - - bool last_has_dock = has_dock; - if (docking_point_client.call(get_docking_point_srv)) { - has_dock = true; - dockX = get_docking_point_srv.response.docking_pose.position.x; - dockY = get_docking_point_srv.response.docking_pose.position.y; - } else { - has_dock = false; - } - - if (last_has_dock != has_dock) { - ROS_INFO_STREAM("map has a dock: " << (has_dock ? "YES" : "NO")); - } -} - -void publishStatus(const ros::TimerEvent &timer_event) { - if (config.emergency_stop) { - geometry_msgs::Twist emergency_twist; - emergency_twist.linear.x = 0.0; - emergency_twist.linear.y = 0.0; - emergency_twist.linear.z = 0.0; - emergency_twist.angular.x = 0.0; - emergency_twist.angular.y = 0.0; - emergency_twist.angular.z = 0.0; - - cmd_vel_pub.publish(emergency_twist); - } else { - cmd_vel_pub.publish(last_cmd_vel); - } - - fake_mow_status.stamp = ros::Time::now(); - fake_mow_status.mow_enabled = config.mower_running; - fake_mow_status.rain_detected = config.rain; - fake_mow_status.mow_esc_status.temperature_motor = config.temperature_mower; - fake_mow_status.mow_esc_status.status = mower_msgs::ESCStatus::ESC_STATUS_OK; - if (config.mower_error) { - fake_mow_status.mow_esc_status.status = mower_msgs::ESCStatus ::ESC_STATUS_ERROR; - ; - } - if (config.mower_running) { - fake_mow_status.mow_esc_status.status = mower_msgs::ESCStatus::ESC_STATUS_RUNNING; - } - - fake_mow_status.v_battery = config.battery_voltage; - - bool last_is_docked = is_docked; - is_docked = - has_dock && sqrt(pow(pose.pose.pose.position.x - dockX, 2) + pow(pose.pose.pose.position.y - dockY, 2)) < 0.25; - - if (last_is_docked != is_docked) { - ROS_INFO_STREAM("simulation is inside dock: " << (is_docked ? "YES" : "NO")); - } - - if (is_docked) { - // "docked", simulate charge voltage. - fake_mow_status.v_charge = 29.4; - } else { - // Not docked, use manual charge flag for charging status - fake_mow_status.v_charge = config.is_charging ? 29.5 : 0.0; - } - if (config.wheels_stalled) { - fake_mow_status.left_esc_status.status = mower_msgs::ESCStatus ::ESC_STATUS_STALLED; - fake_mow_status.right_esc_status.status = mower_msgs::ESCStatus ::ESC_STATUS_STALLED; - } else { - fake_mow_status.left_esc_status.status = mower_msgs::ESCStatus::ESC_STATUS_OK; - fake_mow_status.right_esc_status.status = mower_msgs::ESCStatus::ESC_STATUS_OK; - } - fake_mow_status.emergency = config.emergency_stop; - - status_pub.publish(fake_mow_status); -} - -void reconfigureCB(mower_simulation::MowerSimulationConfig &c, uint32_t level) { - config = c; -} - -void velReceived(const geometry_msgs::Twist::ConstPtr &msg) { - last_cmd_vel = *msg; -} -void odomReceived(const nav_msgs::Odometry::ConstPtr &msg) { - pose.header = msg->header; - pose.pose = msg->pose; - pose.orientation_valid = true; - pose.orientation_accuracy = 0.1; - pose.position_accuracy = 0.02; - pose.source = xbot_msgs::AbsolutePose::SOURCE_SENSOR_FUSION; - pose.flags = xbot_msgs::AbsolutePose::FLAG_SENSOR_FUSION_RECENT_ABSOLUTE_POSE; - - tf2::Quaternion q; - tf2::fromMsg(msg->pose.pose.orientation, q); - - tf2::Matrix3x3 m(q); - double unused1, unused2, yaw; - - m.getRPY(unused1, unused2, yaw); - - pose.vehicle_heading = yaw; - pose.motion_heading = yaw; - pose.motion_vector_valid = false; - - pose_pub.publish(pose); -} - -bool setPose(xbot_positioning::SetPoseSrvRequest &req, xbot_positioning::SetPoseSrvResponse &res) { - double yaw; - { - tf2::Quaternion q; - tf2::fromMsg(req.robot_pose.orientation, q); - - tf2::Matrix3x3 m(q); - double unused1, unused2; - - m.getRPY(unused1, unused2, yaw); - } - - tf2::Quaternion q(0.0, 0.0, yaw); - - initialPoseMsg.header.stamp = ros::Time::now(); - initialPoseMsg.header.frame_id = "base_link"; - initialPoseMsg.header.seq++; - initialPoseMsg.pose.pose.position.x = req.robot_pose.position.x; - initialPoseMsg.pose.pose.position.y = req.robot_pose.position.y; - initialPoseMsg.pose.pose.position.z = 0; - initialPoseMsg.pose.pose.orientation = tf2::toMsg(q); - - initial_pose_publisher.publish(initialPoseMsg); - - return true; -} - int main(int argc, char **argv) { ros::init(argc, argv, "mower_simulation"); @@ -208,53 +62,30 @@ int main(int argc, char **argv) { ros::NodeHandle paramNh("~"); reconfig_server = new dynamic_reconfigure::Server(paramNh); - reconfig_server->setCallback(reconfigureCB); + // reconfig_server->setCallback(reconfigureCB); docking_point_client = n.serviceClient("mower_map_service/get_docking_point"); - initial_pose_publisher = n.advertise("initialpose", 1); - - /* - This introduces more issues than it solves.. - - ROS_INFO("Waiting for map service"); - if (!docking_point_client.waitForExistence(ros::Duration(60.0, 0.0))) { - ROS_ERROR("No map client found; we can't set robot's initial pose."); - } else { - mower_map::GetDockingPointSrv get_docking_point_srv; - geometry_msgs::PoseWithCovarianceStamped docking_pose_stamped; - - docking_point_client.call(get_docking_point_srv); - docking_pose_stamped.pose.pose = get_docking_point_srv.response.docking_pose; - docking_pose_stamped.header.frame_id = "map"; - docking_pose_stamped.header.stamp = ros::Time::now(); - initial_pose_publisher.publish(docking_pose_stamped); - } - */ + xbot::service::system::initSystem(); + xbot::service::Io::start(); - fake_mow_status.mower_status = mower_msgs::Status::MOWER_STATUS_OK; - fake_mow_status.v_charge = 0.0; - fake_mow_status.v_battery = 29.0; - fake_mow_status.stamp = ros::Time::now(); - fake_mow_status.left_esc_status.status = fake_mow_status.right_esc_status.status = - fake_mow_status.mow_esc_status.status = mower_msgs::ESCStatus::ESC_STATUS_OK; - fake_mow_status.mow_esc_status.temperature_motor = 50; - fake_mow_status.emergency = true; - config.emergency_stop = true; + SimRobot robot{paramNh}; - status_pub = n.advertise("mower/status", 1); - cmd_vel_pub = n.advertise("/cmd_vel_out", 1); - pose_pub = n.advertise("/xbot_positioning/xb_pose", 1); + EmergencyService emergency_service{1, robot}; + DiffDriveService diff_drive_service{2, robot}; + MowerService mower_service{3, robot}; + ImuService imu_service{4, robot}; + PowerService power_service{5, robot}; + PositionService gps_position_service{6, robot}; - ros::Subscriber cmd_vel_sub = n.subscribe("/cmd_vel", 0, velReceived, ros::TransportHints().tcpNoDelay(true)); - ros::Subscriber odom_sub = n.subscribe("/mower/odom", 0, odomReceived, ros::TransportHints().tcpNoDelay(true)); - ros::ServiceServer mow_service = n.advertiseService("mower_service/mow_enabled", setMowEnabled); - ros::ServiceServer gps_service = n.advertiseService("xbot_positioning/set_gps_state", setGpsState); - ros::ServiceServer emergency_service = n.advertiseService("mower_service/emergency", setEmergencyStop); - ros::ServiceServer pose_service = n.advertiseService("xbot_positioning/set_robot_pose", setPose); + emergency_service.start(); + diff_drive_service.start(); + mower_service.start(); + imu_service.start(); + power_service.start(); + gps_position_service.start(); - ros::Timer publish_timer = n.createTimer(ros::Duration(0.02), publishStatus); - ros::Timer update_dock_timer = n.createTimer(ros::Duration(1.0), fetchDock); + robot.Start(); ros::spin(); delete (reconfig_server); diff --git a/src/mower_simulation/src/services/diff_drive_service/diff_drive_service.cpp b/src/mower_simulation/src/services/diff_drive_service/diff_drive_service.cpp new file mode 100644 index 00000000..a446e49a --- /dev/null +++ b/src/mower_simulation/src/services/diff_drive_service/diff_drive_service.cpp @@ -0,0 +1,42 @@ +// +// Created by clemens on 26.07.24. +// + +#include "diff_drive_service.hpp" + +bool DiffDriveService::Configure() { + // Check, if configuration is valid, if not retry + if (!WheelDistance.valid || !WheelTicksPerMeter.valid || WheelDistance.value == 0 || + WheelTicksPerMeter.value == 0.0) { + return false; + } + // It's fine, we don't actually need to configure anything + return true; +} +void DiffDriveService::OnStart() { +} +void DiffDriveService::OnCreate() { +} +void DiffDriveService::OnStop() { + robot_.SetControlTwist(0, 0); +} + +void DiffDriveService::tick() { + StartTransaction(); + SendLeftESCStatus(200u); + SendRightESCStatus(200u); + double twist[6]{0}; + robot_.GetTwist(twist[0],twist[5]); + SendActualTwist(twist, sizeof(twist)/sizeof(double)); + CommitTransaction(); +} + +bool DiffDriveService::OnControlTwistChanged(const double* new_value, uint32_t length) { + if (length != 6) return false; + // we can only do forward and rotation around one axis + const auto linear = static_cast(new_value[0]); + const auto angular = static_cast(new_value[5]); + + robot_.SetControlTwist(linear, angular); + return true; +} diff --git a/src/mower_simulation/src/services/diff_drive_service/diff_drive_service.hpp b/src/mower_simulation/src/services/diff_drive_service/diff_drive_service.hpp new file mode 100644 index 00000000..f5d5c85c --- /dev/null +++ b/src/mower_simulation/src/services/diff_drive_service/diff_drive_service.hpp @@ -0,0 +1,37 @@ +// +// Created by clemens on 26.07.24. +// + +#ifndef DIFF_DRIVE_SERVICE_HPP +#define DIFF_DRIVE_SERVICE_HPP + +#include +#include "../../SimRobot.h" + +class DiffDriveService : public DiffDriveServiceBase { + + public: + explicit DiffDriveService(uint16_t service_id, SimRobot &robot) + : DiffDriveServiceBase(service_id, 20000), robot_(robot) { + } + + void OnMowerStatusChanged(uint32_t new_status); + + protected: + bool Configure() override; + void OnStart() override; + void OnCreate() override; + void OnStop() override; + + private: + SimRobot &robot_; + void tick() override; + + void SetDuty(); + void ProcessStatusUpdate(); + + protected: + bool OnControlTwistChanged(const double* new_value, uint32_t length) override; +}; + +#endif // DIFF_DRIVE_SERVICE_HPP diff --git a/src/mower_simulation/src/services/emergency_service/emergency_service.cpp b/src/mower_simulation/src/services/emergency_service/emergency_service.cpp new file mode 100644 index 00000000..f32c2555 --- /dev/null +++ b/src/mower_simulation/src/services/emergency_service/emergency_service.cpp @@ -0,0 +1,61 @@ +// +// Created by clemens on 26.07.24. +// + +#include "emergency_service.hpp" + +bool EmergencyService::Configure() { + // No config needed + return true; +} +void EmergencyService::OnStart() { + robot_.SetEmergency(true, "Boot"); +} + +void EmergencyService::OnStop() { + robot_.SetEmergency(true, "Service Stopped"); +} +void EmergencyService::OnCreate() {} + +void EmergencyService::tick() { + /*// Get the current emergency state + chMtxLock(&mower_status_mutex); + uint32_t status_copy = mower_status; + chMtxUnlock(&mower_status_mutex); + bool emergency_latch = (status_copy & MOWER_FLAG_EMERGENCY_LATCH) != 0; + bool emergency_active = (status_copy & MOWER_FLAG_EMERGENCY_ACTIVE) != 0; + // Check timeout, but only overwrite if no emergency is currently active + // reasoning is that we want to keep the original reason and not overwrite + // with "timeout" + if (!emergency_latch && + chVTTimeElapsedSinceX(last_clear_emergency_message_) > TIME_S2I(1)) { + emergency_reason = "Timeout"; + // set the emergency and notify services + chMtxLock(&mower_status_mutex); + mower_status |= MOWER_FLAG_EMERGENCY_LATCH; + chMtxUnlock(&mower_status_mutex); + chEvtBroadcastFlags(&mower_events, MOWER_EVT_EMERGENCY_CHANGED); + // The last flags did not have emergency yet, so need to set it here as well + emergency_latch = true; + }*/ + + bool emergency_active, emergency_latch; + robot_.GetEmergencyState(emergency_active, emergency_latch, emergency_reason); + + StartTransaction(); + SendEmergencyActive(emergency_active); + + SendEmergencyLatch(emergency_latch); + SendEmergencyReason(emergency_reason.c_str(), emergency_reason.length()); + CommitTransaction(); +} + +bool EmergencyService::OnSetEmergencyChanged(const uint8_t& new_value) { + if(new_value) { + robot_.SetEmergency(true, "High Level"); + } else { + robot_.ResetEmergency(); + } + + return true; +} diff --git a/src/mower_simulation/src/services/emergency_service/emergency_service.hpp b/src/mower_simulation/src/services/emergency_service/emergency_service.hpp new file mode 100644 index 00000000..a03325da --- /dev/null +++ b/src/mower_simulation/src/services/emergency_service/emergency_service.hpp @@ -0,0 +1,38 @@ +// +// Created by clemens on 26.07.24. +// + +#ifndef EMERGENCY_SERVICE_HPP +#define EMERGENCY_SERVICE_HPP + +#include + +#include + +#include "../../SimRobot.h" +class EmergencyService : public EmergencyServiceBase { + private: + public: + explicit EmergencyService(uint16_t service_id, SimRobot &robot) + : EmergencyServiceBase(service_id, 100000), robot_(robot) { + } + + protected: + bool Configure() override; + void OnStart() override; + void OnStop() override; + void OnCreate() override; + + private: + void tick() override; + + SimRobot &robot_; + + ros::Time last_clear_emergency_message_{0}; + std::string emergency_reason{"Boot"}; + + protected: + bool OnSetEmergencyChanged(const uint8_t &new_value) override; +}; + +#endif // EMERGENCY_SERVICE_HPP diff --git a/src/mower_simulation/src/services/imu_service/imu_service.cpp b/src/mower_simulation/src/services/imu_service/imu_service.cpp new file mode 100644 index 00000000..d0cd39e2 --- /dev/null +++ b/src/mower_simulation/src/services/imu_service/imu_service.cpp @@ -0,0 +1,20 @@ +// +// Created by clemens on 31.07.24. +// + +#include "imu_service.hpp" + +bool ImuService::Configure() { return true; } +void ImuService::OnStart() {} +void ImuService::OnStop() {} +void ImuService::OnCreate() { + +} +void ImuService::tick() { + double axes[9]{}; + + double unused; + robot_.GetTwist(unused, axes[5]); + + SendAxes(axes, 9); +} diff --git a/src/mower_simulation/src/services/imu_service/imu_service.hpp b/src/mower_simulation/src/services/imu_service/imu_service.hpp new file mode 100644 index 00000000..606efaf2 --- /dev/null +++ b/src/mower_simulation/src/services/imu_service/imu_service.hpp @@ -0,0 +1,27 @@ +// +// Created by clemens on 31.07.24. +// + +#ifndef IMU_SERVICE_HPP +#define IMU_SERVICE_HPP + +#include +#include "../../SimRobot.h" + +class ImuService : public ImuServiceBase { + public: + explicit ImuService(const uint16_t service_id, SimRobot &robot) + : ImuServiceBase(service_id, 10000), robot_(robot) {} + + protected: + bool Configure() override; + void OnStart() override; + void OnStop() override; + void OnCreate() override; + + private: + SimRobot &robot_; + void tick() override; +}; + +#endif // IMU_SERVICE_HPP diff --git a/src/mower_simulation/src/services/mower_service/mower_service.cpp b/src/mower_simulation/src/services/mower_service/mower_service.cpp new file mode 100644 index 00000000..b0cdbfd8 --- /dev/null +++ b/src/mower_simulation/src/services/mower_service/mower_service.cpp @@ -0,0 +1,28 @@ +// +// Created by clemens on 31.07.24. +// + +#include "mower_service.hpp" + + +bool MowerService::Configure() { + // No configuration needed + return true; +} +void MowerService::OnCreate() { +} +void MowerService::OnStart() { } +void MowerService::OnStop() { } + +void MowerService::tick() { +} + + +bool MowerService::OnMowerEnabledChanged(const uint8_t& new_value) { + + return true; +} + +MowerService::MowerService(const uint16_t service_id, SimRobot &robot) + : MowerServiceBase(service_id, 100000000), robot_(robot) { +} diff --git a/src/mower_simulation/src/services/mower_service/mower_service.hpp b/src/mower_simulation/src/services/mower_service/mower_service.hpp new file mode 100644 index 00000000..b1fb9d4b --- /dev/null +++ b/src/mower_simulation/src/services/mower_service/mower_service.hpp @@ -0,0 +1,30 @@ +// +// Created by clemens on 31.07.24. +// + +#ifndef MOWER_SERVICE_HPP +#define MOWER_SERVICE_HPP + +#include +#include "../../SimRobot.h" + +class MowerService : public MowerServiceBase { + public: + explicit MowerService(const uint16_t service_id, SimRobot &robot_); + + + protected: + bool Configure() override; + void OnCreate() override; + void OnStart() override; + void OnStop() override; + + private: + SimRobot &robot_; + void tick() override; + + protected: + bool OnMowerEnabledChanged(const uint8_t& new_value) override; +}; + +#endif // MOWER_SERVICE_HPP diff --git a/src/mower_simulation/src/services/position_service/position_service.cpp b/src/mower_simulation/src/services/position_service/position_service.cpp new file mode 100644 index 00000000..19299f95 --- /dev/null +++ b/src/mower_simulation/src/services/position_service/position_service.cpp @@ -0,0 +1,37 @@ +// +// Created by clemens on 02.08.24. +// + +#include "position_service.hpp" +bool PositionService::Configure() { + // We're always happy + return true; +} +void PositionService::OnStart() { +} +void PositionService::OnCreate() { +} +void PositionService::OnStop() { +} +void PositionService::tick() { + StartTransaction(); + std::string fix_type = "FIX"; + SendFixType(fix_type.c_str(), fix_type.length()); + SendPositionAccuracy(0.05); + double xyz[3]{}; + double headingAndAccuracy[2]; + headingAndAccuracy[1] = M_PI / 10.0; + double vx, vr; + robot_.GetTwist(vx, vr); + robot_.GetPosition(xyz[0], xyz[1], headingAndAccuracy[0]); + SendPositionXYZ(xyz, 3); + SendMotionHeadingAndAccuracy(headingAndAccuracy, 2); + double motion_vector_xyz[3]{}; + motion_vector_xyz[0] = vx * cos(headingAndAccuracy[0]); + motion_vector_xyz[1] = vx * sin(headingAndAccuracy[0]); + SendMotionVectorXYZ(motion_vector_xyz,3); + CommitTransaction(); +} +bool PositionService::OnRTCMChanged(const uint8_t* new_value, uint32_t length) { + return true; +} diff --git a/src/mower_simulation/src/services/position_service/position_service.hpp b/src/mower_simulation/src/services/position_service/position_service.hpp new file mode 100644 index 00000000..c26f4abe --- /dev/null +++ b/src/mower_simulation/src/services/position_service/position_service.hpp @@ -0,0 +1,33 @@ +// +// Created by clemens on 02.08.24. +// + +#ifndef POSITION_SERVICE_HPP +#define POSITION_SERVICE_HPP +#include +#include "../../SimRobot.h" + +class PositionService : public PositionServiceBase { +public: + explicit PositionService(uint16_t service_id, SimRobot &robot) + : PositionServiceBase(service_id, 200000), robot_(robot) { + } +protected: + bool Configure() override; + void OnStart() override; + void OnCreate() override; + void OnStop() override; + + private: + void tick() override; + + protected: + bool OnRTCMChanged(const uint8_t *new_value, uint32_t length) override; + + +private: + SimRobot &robot_; + +}; + +#endif // POSITION_SERVICE_HPP diff --git a/src/mower_simulation/src/services/power_service/power_service.cpp b/src/mower_simulation/src/services/power_service/power_service.cpp new file mode 100644 index 00000000..288ec6a6 --- /dev/null +++ b/src/mower_simulation/src/services/power_service/power_service.cpp @@ -0,0 +1,35 @@ +// +// Created by clemens on 09.09.24. +// + +#include "power_service.hpp" + + +PowerService::PowerService(uint16_t service_id, SimRobot &robot) + : PowerServiceBase(service_id, 200000), robot_(robot) {} +bool PowerService::Configure() { return true; } + +void PowerService::OnStart() { } +void PowerService::OnCreate() {} +void PowerService::OnStop() {} + +void PowerService::tick() { + bool is_charging; + double charging_time, charge_volts, battery_volts, charge_current; + std::string charge_state; + robot_.GetIsCharging(is_charging, charging_time, charge_state, charge_volts, battery_volts, charge_current); + + + // Send the sensor values + StartTransaction(); + SendBatteryVoltage(battery_volts); + SendChargeVoltage(charge_volts); + SendChargeCurrent(charge_current); + SendChargerEnabled(true); + SendChargingStatus(charge_state.c_str(), charge_state.length()); + CommitTransaction(); +} +bool PowerService::OnChargingAllowedChanged(const uint8_t& new_value) { + (void)new_value; + return true; +} diff --git a/src/mower_simulation/src/services/power_service/power_service.hpp b/src/mower_simulation/src/services/power_service/power_service.hpp new file mode 100644 index 00000000..18759417 --- /dev/null +++ b/src/mower_simulation/src/services/power_service/power_service.hpp @@ -0,0 +1,42 @@ +// +// Created by clemens on 09.09.24. +// + +#ifndef POWER_SERVICE_HPP +#define POWER_SERVICE_HPP + +#include +#include "../../SimRobot.h" + +class PowerService : public PowerServiceBase { + public: + explicit PowerService(uint16_t service_id, SimRobot &robot); + + protected: + bool Configure() override; + void OnStart() override; + void OnCreate() override; + void OnStop() override; + + private: + static constexpr auto CHARGE_STATUS_ERROR = "Error"; + static constexpr auto CHARGE_STATUS_FAULT = "Error (Fault)"; + static constexpr auto CHARGE_STATUS_CHARGER_NOT_FOUND = "Charger Comms Error"; + static constexpr auto CHARGE_STATUS_NOT_CHARGING = "Not Charging"; + static constexpr auto CHARGE_STATUS_PRE_CHARGE = "Pre Charge"; + static constexpr auto CHARGE_STATUS_TRICKLE = "Trickle Charge"; + static constexpr auto CHARGE_STATUS_CC = "Fast Charge (CC)"; + static constexpr auto CHARGE_STATUS_CV = "Taper Charge (CV)"; + static constexpr auto CHARGE_STATUS_TOP_OFF = "Top Off"; + static constexpr auto CHARGE_STATUS_DONE = "Done"; + SimRobot &robot_; + + void tick() override; + + + + protected: + bool OnChargingAllowedChanged(const uint8_t& new_value) override; +}; + +#endif // POWER_SERVICE_HPP diff --git a/src/open_mower/CMakeLists.txt b/src/open_mower/CMakeLists.txt index fb2e20b6..2ca644f3 100644 --- a/src/open_mower/CMakeLists.txt +++ b/src/open_mower/CMakeLists.txt @@ -8,8 +8,7 @@ project(open_mower) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS - mower_msgs - mower_simulation + mower_msgs ) ## System dependencies are found with CMake's conventions @@ -102,10 +101,10 @@ find_package(catkin REQUIRED COMPONENTS ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( -# INCLUDE_DIRS include -# LIBRARIES open_mower -# CATKIN_DEPENDS mower_msgs mower_simulation -# DEPENDS system_lib + # INCLUDE_DIRS include + # LIBRARIES open_mower + # CATKIN_DEPENDS mower_msgs mower_simulation + # DEPENDS system_lib ) ########### @@ -115,8 +114,8 @@ catkin_package( ## Specify additional locations of header files ## Your package locations should be listed before other locations include_directories( -# include - ${catkin_INCLUDE_DIRS} + # include + ${catkin_INCLUDE_DIRS} ) ## Declare a C++ library diff --git a/src/open_mower/launch/include/_comms.launch b/src/open_mower/launch/include/_comms.launch index 87cb3ac9..d55e1a10 100644 --- a/src/open_mower/launch/include/_comms.launch +++ b/src/open_mower/launch/include/_comms.launch @@ -4,17 +4,31 @@ --> + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - diff --git a/src/open_mower/launch/include/_localization.launch b/src/open_mower/launch/include/_localization.launch index 9e06c4cf..44fa9d3e 100644 --- a/src/open_mower/launch/include/_localization.launch +++ b/src/open_mower/launch/include/_localization.launch @@ -1,37 +1,12 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + diff --git a/src/open_mower/launch/include/_teleop.launch b/src/open_mower/launch/include/_teleop.launch index 1439ac8b..3fbab3a3 100644 --- a/src/open_mower/launch/include/_teleop.launch +++ b/src/open_mower/launch/include/_teleop.launch @@ -6,7 +6,7 @@ - + diff --git a/src/open_mower/params/hardware_specific/Lubluelu/default_environment.sh b/src/open_mower/params/hardware_specific/Lubluelu/default_environment.sh new file mode 100644 index 00000000..a79d03f6 --- /dev/null +++ b/src/open_mower/params/hardware_specific/Lubluelu/default_environment.sh @@ -0,0 +1,12 @@ +# Set default GPS antenna offset +export OM_ANTENNA_OFFSET_X=${OM_ANTENNA_OFFSET_X:-0.0} +export OM_ANTENNA_OFFSET_Y=${OM_ANTENNA_OFFSET_Y:-0.0} + +# Set distance between wheels in m +export OM_WHEEL_DISTANCE_M=${WHEEL_DISTANCE_M:-0.206} + +# Set default ticks/m +export OM_WHEEL_TICKS_PER_M=${OM_WHEEL_TICKS_PER_M:-3000.0} + +export OM_NO_GPS=True +export OM_V2=True diff --git a/src/open_mower/rviz/record_map.rviz b/src/open_mower/rviz/record_map.rviz index df3c00bb..b72ff176 100644 --- a/src/open_mower/rviz/record_map.rviz +++ b/src/open_mower/rviz/record_map.rviz @@ -12,8 +12,12 @@ Panels: - /Odometry2/Covariance1 - /MarkerArray1 - /MarkerArray2 + - /TF2 + - /TF2/Frames1 + - /LaserScan1 + - /LaserScan2 Splitter Ratio: 0.5 - Tree Height: 719 + Tree Height: 1174 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -29,10 +33,9 @@ Panels: Name: Views Splitter Ratio: 0.5 - Class: rviz/Time - Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: LaserScan Preferences: PromptSaveOnExit: true Toolbars: @@ -129,14 +132,12 @@ Visualization Manager: Unreliable: false Value: false - Class: rviz/TF - Enabled: true + Enabled: false + Filter (blacklist): "" + Filter (whitelist): "" Frame Timeout: 15 Frames: All Enabled: true - base_link: - Value: true - map: - Value: true Marker Alpha: 1 Marker Scale: 1 Name: TF @@ -144,17 +145,15 @@ Visualization Manager: Show Axes: true Show Names: true Tree: - map: - base_link: - {} + {} Update Interval: 0 - Value: true + Value: false - Class: rviz/MarkerArray Enabled: true Marker Topic: /mower_map_service/map_viz Name: MarkerArray Namespaces: - mower_map_service: true + {} Queue Size: 100 Value: true - Alpha: 0.699999988079071 @@ -357,7 +356,7 @@ Visualization Manager: Marker Topic: /mower_map_service/map_viz Name: MarkerArray Namespaces: - mower_map_service: true + {} Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -365,7 +364,7 @@ Visualization Manager: Marker Topic: /slic3r_coverage_planner/path_marker_array Name: MarkerArray Namespaces: - mower_map_service_lines: true + {} Queue Size: 100 Value: true - Alpha: 1 @@ -384,11 +383,114 @@ Visualization Manager: Topic: /move_base_flex/FTCPlanner/global_point Unreliable: false Value: true + - Class: rviz/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: false + base_link: + Value: true + lidar: + Value: true + map: + Value: true + odom: + Value: false + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + odom: + base_link: + lidar: + {} + Update Interval: 0 + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 2 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Spheres + Topic: /ll/lidar + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 0; 255; 0 + Color Transformer: "" + Decay Time: 2 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Spheres + Topic: /slam_toolbox/karto_scan_visualization + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /slam_toolbox/karto_graph_visualization + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Default Light: true - Fixed Frame: map + Fixed Frame: odom Frame Rate: 30 Name: root Tools: @@ -412,7 +514,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 17.372535705566406 + Distance: 9.762353897094727 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -420,25 +522,25 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 24.136079788208008 - Y: -8.17182445526123 - Z: 0.4961405396461487 + X: -0.009702570736408234 + Y: 0.011223136447370052 + Z: 0 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.5647963285446167 + Pitch: 1.5047966241836548 Target Frame: - Yaw: 0.485392302274704 + Yaw: 3.158125877380371 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1016 + Height: 1465 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001930000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005a00000003efc0100000002fb0000000800540069006d00650100000000000005a0000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002f20000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001930000051ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000051f000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000051ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000051f000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000008d10000003efc0100000002fb0000000800540069006d00650100000000000008d10000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000006230000051f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -447,6 +549,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1440 - X: 3274 - Y: 439 + Width: 2257 + X: 2699 + Y: 330