From 3af3eba799c9c795248da1962817df6f35811b04 Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Wed, 24 Jul 2024 15:02:18 +0200 Subject: [PATCH 01/29] Refactoring: Remove legacy localization (#128) --- src/mower_logic/CMakeLists.txt | 8 - .../src/mag_calibration/mag_calibration.cpp | 90 ---- .../src/mower_odometry/mower_odometry.cpp | 453 ------------------ .../launch/include/_localization.launch | 44 +- 4 files changed, 9 insertions(+), 586 deletions(-) delete mode 100644 src/mower_logic/src/mag_calibration/mag_calibration.cpp delete mode 100644 src/mower_logic/src/mower_odometry/mower_odometry.cpp diff --git a/src/mower_logic/CMakeLists.txt b/src/mower_logic/CMakeLists.txt index ab2d1316..520f37ad 100644 --- a/src/mower_logic/CMakeLists.txt +++ b/src/mower_logic/CMakeLists.txt @@ -185,14 +185,6 @@ add_executable(mower_logic 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/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/open_mower/launch/include/_localization.launch b/src/open_mower/launch/include/_localization.launch index 9e06c4cf..93146fd0 100644 --- a/src/open_mower/launch/include/_localization.launch +++ b/src/open_mower/launch/include/_localization.launch @@ -1,37 +1,11 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + From af64cf7cbf7b769ed14b3a0d6a4fb4779b044528 Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Wed, 24 Jul 2024 16:24:28 +0200 Subject: [PATCH 02/29] wip wip --- .gitignore | 1 + .gitmodules | 3 ++ services/diff_drive_service.json | 19 ++++++++++++ services/emergency_service.json | 30 +++++++++++++++++++ src/lib/xbot_framework | 1 + .../CMakeLists.txt | 20 ++++++++----- .../package.xml | 5 ++-- .../src/COBS.h | 0 .../src/ll_datatypes.h | 0 .../src/mower_comms.cpp | 2 +- 10 files changed, 71 insertions(+), 10 deletions(-) create mode 100644 services/diff_drive_service.json create mode 100644 services/emergency_service.json create mode 160000 src/lib/xbot_framework rename src/{mower_comms => mower_comms_v1}/CMakeLists.txt (92%) rename src/{mower_comms => mower_comms_v1}/package.xml (91%) rename src/{mower_comms => mower_comms_v1}/src/COBS.h (100%) rename src/{mower_comms => mower_comms_v1}/src/ll_datatypes.h (100%) rename src/{mower_comms => mower_comms_v1}/src/mower_comms.cpp (99%) 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/services/diff_drive_service.json b/services/diff_drive_service.json new file mode 100644 index 00000000..4a242db7 --- /dev/null +++ b/services/diff_drive_service.json @@ -0,0 +1,19 @@ +{ + "inputs": [ + { + "id": 0, + "name": "Twist", + "type": "double[6]" + } + ], + "outputs": [ + { + "id": 0, + "name": "TwistOut", + "type": "double[6]" + } + ], + "registers": [], + "type": "DiffDriveService", + "version": 1 +} diff --git a/services/emergency_service.json b/services/emergency_service.json new file mode 100644 index 00000000..0872e8d3 --- /dev/null +++ b/services/emergency_service.json @@ -0,0 +1,30 @@ +{ + "type": "EchoService", + "version": 1, + "inputs": [ + { + "id": 0, + "name": "Set Emergency", + "type": "bool" + } + ], + "outputs": [ + { + "id": 0, + "name": "Emergency Active", + "type": "bool" + }, + { + "id": 1, + "name": "Emergency Latch", + "type": "bool" + }, + { + "id": 2, + "name": "Emergency Reason", + "type": "char[255]" + } + ], + "registers": [ + ] +} diff --git a/src/lib/xbot_framework b/src/lib/xbot_framework new file mode 160000 index 00000000..2aba59da --- /dev/null +++ b/src/lib/xbot_framework @@ -0,0 +1 @@ +Subproject commit 2aba59da90aea1cfb9884ea5a1488884644ed061 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 426f4259..76b8b22b 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) @@ -22,6 +22,9 @@ find_package( Boost ) +add_service(EmergencyService ${CMAKE_CURRENT_SOURCE_DIR}/../../services/emergency_service.json) +add_service(DiffDriveService ${CMAKE_CURRENT_SOURCE_DIR}/../../services/diff_drive_service.json) + ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -113,7 +116,7 @@ find_package( ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include -# LIBRARIES mower_comms +# LIBRARIES mower_comms_v1 # CATKIN_DEPENDS mower_msgs roscpp serial # DEPENDS system_lib ) @@ -131,7 +134,7 @@ include_directories( ## 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 @@ -142,7 +145,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 @@ -159,14 +162,17 @@ 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} + EmergencyService + DiffDriveService +) ############# ## 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 99% rename from src/mower_comms/src/mower_comms.cpp rename to src/mower_comms_v1/src/mower_comms.cpp index 545a1d0d..1fbb638f 100644 --- a/src/mower_comms/src/mower_comms.cpp +++ b/src/mower_comms_v1/src/mower_comms.cpp @@ -431,7 +431,7 @@ void handleLowLevelIMU(struct ll_imu *imu) { } 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; From 0f5c79600781394b1a468777de11a92e556b51f5 Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Thu, 25 Jul 2024 17:59:14 +0200 Subject: [PATCH 03/29] wip --- services/emergency_service.json | 11 +- services/imu_service.json | 13 + src/lib/xbot_framework | 2 +- src/mower_comms_v2/CMakeLists.txt | 231 ++++++++++++++++++ src/mower_comms_v2/package.xml | 38 +++ .../src/EmergencyServiceInterface.cpp | 5 + .../src/EmergencyServiceInterface.h | 22 ++ src/mower_comms_v2/src/mower_comms.cpp | 201 +++++++++++++++ 8 files changed, 516 insertions(+), 7 deletions(-) create mode 100644 services/imu_service.json create mode 100644 src/mower_comms_v2/CMakeLists.txt create mode 100644 src/mower_comms_v2/package.xml create mode 100644 src/mower_comms_v2/src/EmergencyServiceInterface.cpp create mode 100644 src/mower_comms_v2/src/EmergencyServiceInterface.h create mode 100644 src/mower_comms_v2/src/mower_comms.cpp diff --git a/services/emergency_service.json b/services/emergency_service.json index 0872e8d3..b412d215 100644 --- a/services/emergency_service.json +++ b/services/emergency_service.json @@ -1,23 +1,23 @@ { - "type": "EchoService", + "type": "EmergencyService", "version": 1, "inputs": [ { "id": 0, "name": "Set Emergency", - "type": "bool" + "type": "uint8_t" } ], "outputs": [ { "id": 0, "name": "Emergency Active", - "type": "bool" + "type": "uint8_t" }, { "id": 1, "name": "Emergency Latch", - "type": "bool" + "type": "uint8_t" }, { "id": 2, @@ -25,6 +25,5 @@ "type": "char[255]" } ], - "registers": [ - ] + "registers": [] } diff --git a/services/imu_service.json b/services/imu_service.json new file mode 100644 index 00000000..2aff2c51 --- /dev/null +++ b/services/imu_service.json @@ -0,0 +1,13 @@ +{ + "type": "IMUService", + "version": 1, + "inputs": [], + "outputs": [ + { + "id": 0, + "name": "Axes", + "type": "double[9]" + } + ], + "registers": [] +} diff --git a/src/lib/xbot_framework b/src/lib/xbot_framework index 2aba59da..ff88b921 160000 --- a/src/lib/xbot_framework +++ b/src/lib/xbot_framework @@ -1 +1 @@ -Subproject commit 2aba59da90aea1cfb9884ea5a1488884644ed061 +Subproject commit ff88b921d0a53d8b738c0833c9ab257813b2d51d diff --git a/src/mower_comms_v2/CMakeLists.txt b/src/mower_comms_v2/CMakeLists.txt new file mode 100644 index 00000000..38ed760e --- /dev/null +++ b/src/mower_comms_v2/CMakeLists.txt @@ -0,0 +1,231 @@ +cmake_minimum_required(VERSION 3.0.2) +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 + serial + xesc_driver + xesc_msgs + xesc_interface + xbot_msgs +# xbot_framework +) + +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 +) + +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) + + +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..7a95e360 --- /dev/null +++ b/src/mower_comms_v2/package.xml @@ -0,0 +1,38 @@ + + + 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 + serial + xesc_driver + ublox_msgs + xbot_msgs + mower_msgs + roscpp + serial + xesc_driver + xbot_msgs + mower_msgs + roscpp + serial + xesc_driver + xesc_msgs + + + + + + + + diff --git a/src/mower_comms_v2/src/EmergencyServiceInterface.cpp b/src/mower_comms_v2/src/EmergencyServiceInterface.cpp new file mode 100644 index 00000000..6128245c --- /dev/null +++ b/src/mower_comms_v2/src/EmergencyServiceInterface.cpp @@ -0,0 +1,5 @@ +// +// Created by clemens on 25.07.24. +// + +#include "EmergencyServiceInterface.h" diff --git a/src/mower_comms_v2/src/EmergencyServiceInterface.h b/src/mower_comms_v2/src/EmergencyServiceInterface.h new file mode 100644 index 00000000..c700f99a --- /dev/null +++ b/src/mower_comms_v2/src/EmergencyServiceInterface.h @@ -0,0 +1,22 @@ +// +// Created by clemens on 25.07.24. +// + +#ifndef EMERGENCYSERVICEINTERFACE_H +#define EMERGENCYSERVICEINTERFACE_H + +#include + +#include + +class EmergencyServiceInterface : public EmergencyServiceInterfaceBase { + public: + EmergencyServiceInterface(const ros::NodeHandle &nh, uint16_t service_id, const xbot::serviceif::Context &ctx) + : EmergencyServiceInterfaceBase(service_id, ctx), nh(nh) { + } + + private: + const ros::NodeHandle &nh; +}; + +#endif // EMERGENCYSERVICEINTERFACE_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..ad1d8cf5 --- /dev/null +++ b/src/mower_comms_v2/src/mower_comms.cpp @@ -0,0 +1,201 @@ +// +// 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 + +#include "boost/crc.hpp" +#include "mower_msgs/EmergencyStopSrv.h" +#include "mower_msgs/HighLevelControlSrv.h" +#include "mower_msgs/HighLevelStatus.h" +#include "mower_msgs/ImuRaw.h" +#include "mower_msgs/MowerControlSrv.h" +#include "ros/ros.h" +#include "sensor_msgs/Imu.h" +#include "sensor_msgs/MagneticField.h" +#include "std_msgs/Bool.h" +#include "std_msgs/Empty.h" + +ros::Publisher status_pub; +ros::Publisher wheel_tick_pub; + +ros::Publisher sensor_imu_pub; + +ros::ServiceClient highLevelClient; + +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"); + + paramNh.getParam("wheel_ticks_per_m", wheel_ticks_per_m); + paramNh.getParam("wheel_distance_m", wheel_distance_m); + + ROS_INFO_STREAM("Wheel ticks [1/m]: " << wheel_ticks_per_m); + ROS_INFO_STREAM("Wheel distance [m]: " << wheel_distance_m); + + speed_l = speed_r = speed_mow = target_speed_mow = 0; + + status_pub = n.advertise("mower/status", 1); + wheel_tick_pub = n.advertise("mower/wheel_ticks", 1); + + sensor_imu_pub = n.advertise("imu/data_raw", 1); + sensor_mag_pub = n.advertise("imu/mag", 1); + ros::ServiceServer mow_service = n.advertiseService("mower_service/mow_enabled", setMowEnabled); + ros::ServiceServer emergency_service = n.advertiseService("mower_service/emergency", setEmergencyStop); + ros::Subscriber cmd_vel_sub = n.subscribe("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.02), publishActuatorsTimerTask); + + size_t buflen = 1000; + uint8_t buffer[buflen]; + uint8_t buffer_decoded[buflen]; + size_t read = 0; + // don't change, we need to wait for arduino to boot before actually sending stuff + ros::Duration retryDelay(5, 0); + ros::AsyncSpinner spinner(1); + spinner.start(); + while (ros::ok()) { + if (!serial_port.isOpen()) { + ROS_INFO_STREAM("connecting serial interface: " << ll_serial_port_name); + allow_send = false; + try { + serial_port.setPort(ll_serial_port_name); + serial_port.setBaudrate(115200); + auto to = serial::Timeout::simpleTimeout(100); + serial_port.setTimeout(to); + serial_port.open(); + + // wait for controller to boot + retryDelay.sleep(); + // this will only be set if no error was set + + allow_send = true; + } catch (std::exception &e) { + retryDelay.sleep(); + ROS_ERROR_STREAM("Error during reconnect."); + } + } + size_t bytes_read = 0; + try { + bytes_read = serial_port.read(buffer + read, 1); + } catch (std::exception &e) { + ROS_ERROR_STREAM("Error reading serial_port. Closing Connection."); + serial_port.close(); + retryDelay.sleep(); + } + if (read + bytes_read >= buflen) { + read = 0; + bytes_read = 0; + ROS_ERROR_STREAM("Prevented buffer overflow. There is a problem with the serial comms."); + } + if (bytes_read) { + if (buffer[read] == 0) { + // end of packet found + size_t data_size = cobs.decode(buffer, read, buffer_decoded); + + // first, check the CRC + if (data_size < 3) { + // We don't even have one byte of data + // (type + crc = 3 bytes already) + ROS_INFO_STREAM("Got empty packet from Low Level Board"); + } else { + // We have at least 1 byte of data, check the CRC + crc.reset(); + // We start at the second byte (ignore the type) and process (data_size- byte for type - 2 bytes for CRC) + // bytes. + crc.process_bytes(buffer_decoded, data_size - 2); + uint16_t checksum = crc.checksum(); + uint16_t received_checksum = *(uint16_t *)(buffer_decoded + data_size - 2); + if (checksum == received_checksum) { + // Packet checksum is OK, process it + switch (buffer_decoded[0]) { + case PACKET_ID_LL_STATUS: + if (data_size == sizeof(struct ll_status)) { + handleLowLevelStatus((struct ll_status *)buffer_decoded); + } else { + ROS_INFO_STREAM("Low Level Board sent a valid packet with the wrong size. Type was STATUS"); + } + break; + case PACKET_ID_LL_IMU: + if (data_size == sizeof(struct ll_imu)) { + handleLowLevelIMU((struct ll_imu *)buffer_decoded); + } else { + ROS_INFO_STREAM("Low Level Board sent a valid packet with the wrong size. Type was IMU"); + } + break; + case PACKET_ID_LL_UI_EVENT: + if (data_size == sizeof(struct ll_ui_event)) { + handleLowLevelUIEvent((struct ll_ui_event *)buffer_decoded); + } else { + ROS_INFO_STREAM("Low Level Board sent a valid packet with the wrong size. Type was UI_EVENT"); + } + break; + case PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ: + case PACKET_ID_LL_HIGH_LEVEL_CONFIG_RSP: + if (data_size == sizeof(struct ll_high_level_config)) { + handleLowLevelConfig((struct ll_high_level_config *)buffer_decoded); + } else { + ROS_INFO_STREAM("Low Level Board sent a valid packet with the wrong size. Type was CONFIG_* (0x" + << std::hex << buffer_decoded[0] << ")"); + } + break; + default: ROS_INFO_STREAM("Got unknown packet from Low Level Board"); break; + } + } else { + ROS_INFO_STREAM("Got invalid checksum from Low Level Board"); + } + } + + read = 0; + } else { + read += bytes_read; + } + } + } + + spinner.stop(); + + if (mow_xesc_interface) { + mow_xesc_interface->setDutyCycle(0.0); + mow_xesc_interface->stop(); + } + left_xesc_interface->setDutyCycle(0.0); + right_xesc_interface->setDutyCycle(0.0); + left_xesc_interface->stop(); + right_xesc_interface->stop(); + + if (mow_xesc_interface) { + delete mow_xesc_interface; + } + delete left_xesc_interface; + delete right_xesc_interface; + + return 0; +} From 85f9f7929faafb4428718a6f065efbe64fa17beb Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Thu, 25 Jul 2024 18:45:00 +0200 Subject: [PATCH 04/29] extracted emergency into its own message --- src/mower_comms_v1/src/mower_comms.cpp | 17 ++++++++++----- .../src/mower_logic/mower_logic.cpp | 21 ++++++++++++++++--- src/mower_msgs/CMakeLists.txt | 1 + src/mower_msgs/msg/Emergency.msg | 4 ++++ src/mower_msgs/msg/Status.msg | 2 -- 5 files changed, 35 insertions(+), 10 deletions(-) create mode 100644 src/mower_msgs/msg/Emergency.msg diff --git a/src/mower_comms_v1/src/mower_comms.cpp b/src/mower_comms_v1/src/mower_comms.cpp index 1fbb638f..67c3b969 100644 --- a/src/mower_comms_v1/src/mower_comms.cpp +++ b/src/mower_comms_v1/src/mower_comms.cpp @@ -17,6 +17,7 @@ // // #include +#include #include #include #include @@ -41,6 +42,7 @@ #include "std_msgs/Empty.h" ros::Publisher status_pub; +ros::Publisher emergency_pub; ros::Publisher wheel_tick_pub; ros::Publisher sensor_imu_pub; @@ -52,6 +54,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; @@ -184,12 +188,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; @@ -198,7 +199,12 @@ void publishStatus() { } // True, if high or low level emergency condition is present - status_msg.emergency = is_emergency(); + 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); status_msg.v_battery = last_ll_status.v_system; status_msg.v_charge = last_ll_status.v_charge; @@ -475,6 +481,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_logic/src/mower_logic/mower_logic.cpp b/src/mower_logic/src/mower_logic/mower_logic.cpp index 6701eb86..91ec0ea3 100644 --- a/src/mower_logic/src/mower_logic/mower_logic.cpp +++ b/src/mower_logic/src/mower_logic/mower_logic.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -75,6 +76,7 @@ ros::Time pose_time(0.0); xbot_msgs::AbsolutePose last_pose; ros::Time status_time(0.0); mower_msgs::Status last_status; +mower_msgs::Emergency last_emergency; ros::Time last_good_gps(0.0); @@ -113,6 +115,11 @@ void setLastGoodGPS(ros::Time time) { last_good_gps = time; } +mower_msgs::Emergency getEmergency() { + std::lock_guard lk{mower_logic_mutex}; + return last_emergency; +} + mower_msgs::Status getStatus() { std::lock_guard lk{mower_logic_mutex}; return last_status; @@ -192,6 +199,11 @@ void poseReceived(const xbot_msgs::AbsolutePose::ConstPtr &msg) { pose_time = ros::Time::now(); } +void emergencyReceived(const mower_msgs::Emergency &msg) { + std::lock_guard lk{mower_logic_mutex}; + last_emergency = msg; +} + void statusReceived(const mower_msgs::Status::ConstPtr &msg) { std::lock_guard lk{mower_logic_mutex}; @@ -397,13 +409,14 @@ bool isGpsGood() { /// @param timer_event void checkSafety(const ros::TimerEvent &timer_event) { const auto last_status = getStatus(); + const auto last_emergency = getEmergency(); const auto last_config = getConfig(); const auto last_pose = getPose(); const auto pose_time = getPoseTime(); const auto status_time = getStatusTime(); const auto last_good_gps = getLastGoodGPS(); - high_level_status.emergency = last_status.emergency; + high_level_status.emergency = last_emergency.latched_emergency; high_level_status.is_charging = last_status.v_charge > 10.0; // Initialize to true, if after all checks it is still true then mower should be enabled. @@ -411,7 +424,7 @@ void checkSafety(const ros::TimerEvent &timer_event) { // 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) { @@ -666,6 +679,8 @@ int main(int argc, char **argv) { 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 emergency_sub = + n->subscribe("/mower/emergency", 0, emergencyReceived, ros::TransportHints().tcpNoDelay(true)); ros::Subscriber pose_sub = n->subscribe("/xbot_positioning/xb_pose", 0, poseReceived, ros::TransportHints().tcpNoDelay(true)); ros::Subscriber joy_cmd = n->subscribe("/joy_vel", 0, joyVelReceived, ros::TransportHints().tcpNoDelay(true)); @@ -803,7 +818,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 (last_emergency.latched_emergency) { ROS_INFO_STREAM("Got emergency, resetting it"); setEmergencyMode(false); break; diff --git a/src/mower_msgs/CMakeLists.txt b/src/mower_msgs/CMakeLists.txt index 8a12a708..3346b105 100644 --- a/src/mower_msgs/CMakeLists.txt +++ b/src/mower_msgs/CMakeLists.txt @@ -13,6 +13,7 @@ add_message_files( ESCStatus.msg HighLevelStatus.msg Perimeter.msg + Emergency.msg ) add_service_files( 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/Status.msg b/src/mower_msgs/msg/Status.msg index eca5b971..ada325e4 100644 --- a/src/mower_msgs/msg/Status.msg +++ b/src/mower_msgs/msg/Status.msg @@ -14,8 +14,6 @@ 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 From dc1fd2a75b593ba6ac1f1444a42b70c7b8caeab5 Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Fri, 26 Jul 2024 13:14:53 +0200 Subject: [PATCH 05/29] wip --- services/diff_drive_service.json | 41 +++- services/mower_service.json | 74 +++++++ services/mower_ui_service.json | 60 +++++ src/lib/xbot_framework | 2 +- src/mower_comms_v2/CMakeLists.txt | 29 ++- .../src/DiffDriveServiceInterface.cpp | 84 +++++++ .../src/DiffDriveServiceInterface.h | 69 ++++++ .../src/EmergencyServiceInterface.cpp | 71 ++++++ .../src/EmergencyServiceInterface.h | 31 ++- .../src/ImuServiceInterface.cpp | 32 +++ src/mower_comms_v2/src/ImuServiceInterface.h | 33 +++ .../src/MowerServiceInterface.cpp | 57 +++++ .../src/MowerServiceInterface.h | 45 ++++ src/mower_comms_v2/src/mower_comms.cpp | 208 +++++------------- src/mower_msgs/msg/Status.msg | 7 +- 15 files changed, 669 insertions(+), 174 deletions(-) create mode 100644 services/mower_service.json create mode 100644 services/mower_ui_service.json create mode 100644 src/mower_comms_v2/src/DiffDriveServiceInterface.cpp create mode 100644 src/mower_comms_v2/src/DiffDriveServiceInterface.h create mode 100644 src/mower_comms_v2/src/ImuServiceInterface.cpp create mode 100644 src/mower_comms_v2/src/ImuServiceInterface.h create mode 100644 src/mower_comms_v2/src/MowerServiceInterface.cpp create mode 100644 src/mower_comms_v2/src/MowerServiceInterface.h diff --git a/services/diff_drive_service.json b/services/diff_drive_service.json index 4a242db7..13351b6d 100644 --- a/services/diff_drive_service.json +++ b/services/diff_drive_service.json @@ -1,19 +1,50 @@ { + "type": "DiffDriveService", + "version": 1, "inputs": [ { "id": 0, - "name": "Twist", + "name": "Control Twist", "type": "double[6]" } ], "outputs": [ { "id": 0, - "name": "TwistOut", + "name": "Actual Twist", "type": "double[6]" + }, + { + "id": 1, + "name": "Left ESC Temperature", + "type": "float" + }, + { + "id": 2, + "name": "Left ESC Current", + "type": "float" + }, + { + "id": 3, + "name": "Right ESC Temperature", + "type": "float" + }, + { + "id": 4, + "name": "Right ESC Current", + "type": "float" } ], - "registers": [], - "type": "DiffDriveService", - "version": 1 + "registers": [ + { + "id": 0, + "name": "Wheel Ticks Per Meter", + "type": "double" + }, + { + "id": 1, + "name": "Wheel Distance", + "type": "double" + } + ] } diff --git a/services/mower_service.json b/services/mower_service.json new file mode 100644 index 00000000..0f92bd55 --- /dev/null +++ b/services/mower_service.json @@ -0,0 +1,74 @@ +{ + "type": "MowerService", + "version": 1, + "inputs": [ + { + "id": 0, + "name": "MowerEnabled", + "type": "uint8_t" + } + ], + "outputs": [ + { + "id": 0, + "name": "Mower Status", + "type": "uint8_t" + }, + { + "id": 1, + "name": "Raspberry Pi Power", + "type": "uint8_t" + }, + { + "id": 2, + "name": "GPS Power", + "type": "uint8_t" + }, + { + "id": 3, + "name": "ESC Power", + "type": "uint8_t" + }, + { + "id": 4, + "name": "Rain Detected", + "type": "uint8_t" + }, + { + "id": 5, + "name": "Charge Voltage", + "type": "float" + }, + { + "id": 6, + "name": "Battery Voltage", + "type": "float" + }, + { + "id": 7, + "name": "Mower Running", + "type": "uint8_t" + }, + { + "id": 8, + "name": "Mower ESC Temperature", + "type": "float" + }, + { + "id": 9, + "name": "Mower Motor Temperature", + "type": "float" + }, + { + "id": 10, + "name": "Mower Motor Current", + "type": "float" + }, + { + "id": 11, + "name": "Mower Motor RPM", + "type": "float" + } + ], + "registers": [] +} diff --git a/services/mower_ui_service.json b/services/mower_ui_service.json new file mode 100644 index 00000000..5acf4d19 --- /dev/null +++ b/services/mower_ui_service.json @@ -0,0 +1,60 @@ +{ + "type": "MowerUiService", + "version": 1, + "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" + } + ] +} diff --git a/src/lib/xbot_framework b/src/lib/xbot_framework index ff88b921..c8d23684 160000 --- a/src/lib/xbot_framework +++ b/src/lib/xbot_framework @@ -1 +1 @@ -Subproject commit ff88b921d0a53d8b738c0833c9ab257813b2d51d +Subproject commit c8d2368471664ff6ce6563ea328dc886848c9476 diff --git a/src/mower_comms_v2/CMakeLists.txt b/src/mower_comms_v2/CMakeLists.txt index 38ed760e..3d03f33c 100644 --- a/src/mower_comms_v2/CMakeLists.txt +++ b/src/mower_comms_v2/CMakeLists.txt @@ -8,15 +8,15 @@ project(mower_comms_v2) ## 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_msgs sensor_msgs - roscpp - serial + roscpp + serial xesc_driver xesc_msgs xesc_interface xbot_msgs -# xbot_framework + # xbot_framework ) find_package( @@ -114,10 +114,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_v2 -# CATKIN_DEPENDS mower_msgs roscpp serial -# DEPENDS system_lib + # INCLUDE_DIRS include + # LIBRARIES mower_comms_v2 + # CATKIN_DEPENDS mower_msgs roscpp serial + # DEPENDS system_lib ) ########### @@ -127,8 +127,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 @@ -165,11 +165,18 @@ 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 ) -target_add_service_interface(mower_comms_v2 IMUServiceInterface ${CMAKE_CURRENT_SOURCE_DIR}/../../services/imu_service.json) +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) add_dependencies(mower_comms_v2 ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) diff --git a/src/mower_comms_v2/src/DiffDriveServiceInterface.cpp b/src/mower_comms_v2/src/DiffDriveServiceInterface.cpp new file mode 100644 index 00000000..c3edd2a7 --- /dev/null +++ b/src/mower_comms_v2/src/DiffDriveServiceInterface.cpp @@ -0,0 +1,84 @@ +// +// Created by clemens on 25.07.24. +// + +#include "DiffDriveServiceInterface.h" + +#include +#include + +bool DiffDriveServiceInterface::OnConfigurationRequested(const std::string& uid) { + StartTransaction(true); + SetRegisterWheelDistance(wheel_distance_); + SetRegisterWheelTicksPerMeter(ticks_per_meter_); + CommitTransaction(); + return true; +} + +void DiffDriveServiceInterface::SendTwist(const geometry_msgs::TwistConstPtr& msg) { + 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::GetEscInfo(bool& left_error, float& left_esc_temperature, float& left_esc_current, + bool& right_error, float& right_esc_temperature, float& right_esc_current) { + std::unique_lock lk{state_mutex_}; + left_error = left_esc_state_.error; + left_esc_temperature = left_esc_state_.temperature; + left_esc_current = left_esc_state_.current; + right_error = right_esc_state_.error; + right_esc_temperature = right_esc_state_.temperature; + right_esc_current = right_esc_state_.current; +} +void DiffDriveServiceInterface::OnActualTwistChanged(const double* new_value, uint32_t length) { + // 3 linear, 3 angular + if (length == 6) { + 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 = new_value; +} +void DiffDriveServiceInterface::OnRightESCCurrentChanged(const float& new_value) { + std::unique_lock lk{state_mutex_}; + right_esc_state_.current = new_value; +} +void DiffDriveServiceInterface::OnLeftESCTemperatureChanged(const float& new_value) { + std::unique_lock lk{state_mutex_}; + left_esc_state_.temperature = new_value; +} +void DiffDriveServiceInterface::OnServiceConnected(const std::string& uid) { + std::unique_lock lk{state_mutex_}; + left_esc_state_.error = false; + right_esc_state_.error = false; +} +void DiffDriveServiceInterface::OnTransactionStart(uint64_t timestamp) { +} +void DiffDriveServiceInterface::OnTransactionEnd() { +} +void DiffDriveServiceInterface::OnServiceDisconnected(const std::string& uid) { + std::unique_lock lk{state_mutex_}; + left_esc_state_.error = true; + right_esc_state_.error = true; +} diff --git a/src/mower_comms_v2/src/DiffDriveServiceInterface.h b/src/mower_comms_v2/src/DiffDriveServiceInterface.h new file mode 100644 index 00000000..61997116 --- /dev/null +++ b/src/mower_comms_v2/src/DiffDriveServiceInterface.h @@ -0,0 +1,69 @@ +// +// Created by clemens on 25.07.24. +// + +#ifndef DIFFDRIVESERVICEINTERFACE_H +#define DIFFDRIVESERVICEINTERFACE_H + +#include +#include + +#include + +class DiffDriveServiceInterface : public DiffDriveServiceInterfaceBase { + public: + DiffDriveServiceInterface(uint16_t service_id, const xbot::serviceif::Context& ctx, + const ros::Publisher& actual_twist_publisher, double ticks_per_meter, double wheel_distance) + : DiffDriveServiceInterfaceBase(service_id, ctx), + actual_twist_publisher_(actual_twist_publisher), + wheel_distance_(wheel_distance), + ticks_per_meter_(ticks_per_meter) { + } + + bool OnConfigurationRequested(const std::string& uid) override; + + /** + * Convenience function to transmit the twist from a ROS message + * @param msg The ROS message + */ + void SendTwist(const geometry_msgs::TwistConstPtr& msg); + void GetEscInfo(bool& left_error, float& left_esc_temperature, float& left_esc_current, bool& right_error, + float& right_esc_temperature, float& right_esc_current); + + 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; + + private: + void OnServiceConnected(const std::string& uid) override; + void OnTransactionStart(uint64_t timestamp) override; + void OnTransactionEnd() override; + void OnServiceDisconnected(const std::string& uid) 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_; + double wheel_distance_; + double ticks_per_meter_; + + // Store the latest ESC state + struct ESCState { + bool error = true; + float temperature = 0.0; + float current = 0.0; + } left_esc_state_{}, right_esc_state_{}; +}; + +#endif // DIFFDRIVESERVICEINTERFACE_H diff --git a/src/mower_comms_v2/src/EmergencyServiceInterface.cpp b/src/mower_comms_v2/src/EmergencyServiceInterface.cpp index 6128245c..cbe2fc0e 100644 --- a/src/mower_comms_v2/src/EmergencyServiceInterface.cpp +++ b/src/mower_comms_v2/src/EmergencyServiceInterface.cpp @@ -3,3 +3,74 @@ // #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; +} + +bool EmergencyServiceInterface::OnConfigurationRequested(const std::string& uid) { + // 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 && !active_high_level_emergency_) { + // Only clear 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(const std::string& uid) { + 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(const std::string& uid) { + 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 index c700f99a..b00b3ea4 100644 --- a/src/mower_comms_v2/src/EmergencyServiceInterface.h +++ b/src/mower_comms_v2/src/EmergencyServiceInterface.h @@ -9,14 +9,39 @@ #include +namespace sc = std::chrono; + class EmergencyServiceInterface : public EmergencyServiceInterfaceBase { public: - EmergencyServiceInterface(const ros::NodeHandle &nh, uint16_t service_id, const xbot::serviceif::Context &ctx) - : EmergencyServiceInterfaceBase(service_id, ctx), nh(nh) { + 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); + + protected: + bool OnConfigurationRequested(const std::string& uid) 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: - const ros::NodeHandle &nh; + void OnServiceConnected(const std::string& uid) override; + void OnTransactionStart(uint64_t timestamp) override; + void OnTransactionEnd() override; + void OnServiceDisconnected(const std::string& uid) 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/ImuServiceInterface.cpp b/src/mower_comms_v2/src/ImuServiceInterface.cpp new file mode 100644 index 00000000..a6d11fb4 --- /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(const std::string& uid) { + 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(const std::string& uid) { +} +void ImuServiceInterface::OnTransactionStart(uint64_t timestamp) { +} +void ImuServiceInterface::OnTransactionEnd() { +} +void ImuServiceInterface::OnServiceDisconnected(const std::string& uid) { +} diff --git a/src/mower_comms_v2/src/ImuServiceInterface.h b/src/mower_comms_v2/src/ImuServiceInterface.h new file mode 100644 index 00000000..7ea1982d --- /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(const std::string& uid) override; + + protected: + void OnAxesChanged(const double* new_value, uint32_t length) override; + + private: + void OnServiceConnected(const std::string& uid) override; + void OnTransactionStart(uint64_t timestamp) override; + void OnTransactionEnd() override; + void OnServiceDisconnected(const std::string& uid) override; + const ros::Publisher& imu_publisher_; + + sensor_msgs::Imu imu_msg{}; +}; + +#endif // IMUSERVICEINTERFACE_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..85888e5c --- /dev/null +++ b/src/mower_comms_v2/src/MowerServiceInterface.cpp @@ -0,0 +1,57 @@ +// +// Created by clemens on 25.07.24. +// + +#include "MowerServiceInterface.h" + +bool MowerServiceInterface::OnConfigurationRequested(const std::string& uid) { + // No configuration + return true; +} +void MowerServiceInterface::OnMowerStatusChanged(const uint8_t& new_value) { + status_msg_.mower_status = new_value; +} +void MowerServiceInterface::OnRaspberryPiPowerChanged(const uint8_t& new_value) { + status_msg_.raspberry_pi_power = new_value; +} +void MowerServiceInterface::OnGPSPowerChanged(const uint8_t& new_value) { + status_msg_.gps_power = new_value; +} +void MowerServiceInterface::OnESCPowerChanged(const uint8_t& new_value) { + status_msg_.esc_power = new_value; +} +void MowerServiceInterface::OnRainDetectedChanged(const uint8_t& new_value) { + status_msg_.rain_detected = new_value; +} +void MowerServiceInterface::OnChargeVoltageChanged(const float& new_value) { + status_msg_.v_charge = new_value; +} +void MowerServiceInterface::OnBatteryVoltageChanged(const float& new_value) { + status_msg_.v_battery = 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(const std::string& uid) { + status_msg_ = {}; +} +void MowerServiceInterface::OnTransactionStart(uint64_t timestamp) { + status_msg_.stamp = ros::Time::now(); +} +void MowerServiceInterface::OnTransactionEnd() { + status_poblisher_.publish(status_msg_); +} +void MowerServiceInterface::OnServiceDisconnected(const std::string& uid) { +} diff --git a/src/mower_comms_v2/src/MowerServiceInterface.h b/src/mower_comms_v2/src/MowerServiceInterface.h new file mode 100644 index 00000000..63f6e4cc --- /dev/null +++ b/src/mower_comms_v2/src/MowerServiceInterface.h @@ -0,0 +1,45 @@ +// +// 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_poblisher) + : MowerServiceInterfaceBase(service_id, ctx), status_poblisher_(status_poblisher) { + } + + bool OnConfigurationRequested(const std::string& uid) override; + + protected: + void OnMowerStatusChanged(const uint8_t& new_value) override; + void OnRaspberryPiPowerChanged(const uint8_t& new_value) override; + void OnGPSPowerChanged(const uint8_t& new_value) override; + void OnESCPowerChanged(const uint8_t& new_value) override; + void OnRainDetectedChanged(const uint8_t& new_value) override; + void OnChargeVoltageChanged(const float& new_value) override; + void OnBatteryVoltageChanged(const float& 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(const std::string& uid) override; + void OnTransactionStart(uint64_t timestamp) override; + void OnTransactionEnd() override; + void OnServiceDisconnected(const std::string& uid) override; + mower_msgs::Status status_msg_{}; + const ros::Publisher& status_poblisher_; +}; + +#endif // MOWERSERVICEINTERFACE_H diff --git a/src/mower_comms_v2/src/mower_comms.cpp b/src/mower_comms_v2/src/mower_comms.cpp index ad1d8cf5..b083294e 100644 --- a/src/mower_comms_v2/src/mower_comms.cpp +++ b/src/mower_comms_v2/src/mower_comms.cpp @@ -16,36 +16,47 @@ // SOFTWARE. // // -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "boost/crc.hpp" -#include "mower_msgs/EmergencyStopSrv.h" -#include "mower_msgs/HighLevelControlSrv.h" -#include "mower_msgs/HighLevelStatus.h" -#include "mower_msgs/ImuRaw.h" -#include "mower_msgs/MowerControlSrv.h" -#include "ros/ros.h" -#include "sensor_msgs/Imu.h" -#include "sensor_msgs/MagneticField.h" -#include "std_msgs/Bool.h" -#include "std_msgs/Empty.h" +#include +#include +#include +#include +#include +#include + +#include "DiffDriveServiceInterface.h" +#include "EmergencyServiceInterface.h" +#include "ImuServiceInterface.h" +#include "MowerServiceInterface.h" ros::Publisher status_pub; -ros::Publisher wheel_tick_pub; +ros::Publisher emergency_pub; +ros::Publisher actual_twist_pub; ros::Publisher sensor_imu_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; + +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); +} + int main(int argc, char **argv) { ros::init(argc, argv, "mower_comms_v2"); @@ -54,148 +65,43 @@ int main(int argc, char **argv) { 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; + paramNh.getParam("wheel_ticks_per_m", wheel_ticks_per_m); paramNh.getParam("wheel_distance_m", wheel_distance_m); ROS_INFO_STREAM("Wheel ticks [1/m]: " << wheel_ticks_per_m); ROS_INFO_STREAM("Wheel distance [m]: " << wheel_distance_m); - speed_l = speed_r = speed_mow = target_speed_mow = 0; - status_pub = n.advertise("mower/status", 1); - wheel_tick_pub = n.advertise("mower/wheel_ticks", 1); + emergency_pub = n.advertise("mower/emergency", 1); + actual_twist_pub = n.advertise("mower/actual_twist", 1); sensor_imu_pub = n.advertise("imu/data_raw", 1); - sensor_mag_pub = n.advertise("imu/mag", 1); - ros::ServiceServer mow_service = n.advertiseService("mower_service/mow_enabled", setMowEnabled); - ros::ServiceServer emergency_service = n.advertiseService("mower_service/emergency", setEmergencyStop); + // ros::ServiceServer mow_service = n.advertiseService("mower_service/mow_enabled", setMowEnabled); + ros::ServiceServer ros_emergency_service = n.advertiseService("mower_service/emergency", setEmergencyStop); ros::Subscriber cmd_vel_sub = n.subscribe("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.02), publishActuatorsTimerTask); - - size_t buflen = 1000; - uint8_t buffer[buflen]; - uint8_t buffer_decoded[buflen]; - size_t read = 0; - // don't change, we need to wait for arduino to boot before actually sending stuff - ros::Duration retryDelay(5, 0); - ros::AsyncSpinner spinner(1); - spinner.start(); - while (ros::ok()) { - if (!serial_port.isOpen()) { - ROS_INFO_STREAM("connecting serial interface: " << ll_serial_port_name); - allow_send = false; - try { - serial_port.setPort(ll_serial_port_name); - serial_port.setBaudrate(115200); - auto to = serial::Timeout::simpleTimeout(100); - serial_port.setTimeout(to); - serial_port.open(); - - // wait for controller to boot - retryDelay.sleep(); - // this will only be set if no error was set - - allow_send = true; - } catch (std::exception &e) { - retryDelay.sleep(); - ROS_ERROR_STREAM("Error during reconnect."); - } - } - size_t bytes_read = 0; - try { - bytes_read = serial_port.read(buffer + read, 1); - } catch (std::exception &e) { - ROS_ERROR_STREAM("Error reading serial_port. Closing Connection."); - serial_port.close(); - retryDelay.sleep(); - } - if (read + bytes_read >= buflen) { - read = 0; - bytes_read = 0; - ROS_ERROR_STREAM("Prevented buffer overflow. There is a problem with the serial comms."); - } - if (bytes_read) { - if (buffer[read] == 0) { - // end of packet found - size_t data_size = cobs.decode(buffer, read, buffer_decoded); - - // first, check the CRC - if (data_size < 3) { - // We don't even have one byte of data - // (type + crc = 3 bytes already) - ROS_INFO_STREAM("Got empty packet from Low Level Board"); - } else { - // We have at least 1 byte of data, check the CRC - crc.reset(); - // We start at the second byte (ignore the type) and process (data_size- byte for type - 2 bytes for CRC) - // bytes. - crc.process_bytes(buffer_decoded, data_size - 2); - uint16_t checksum = crc.checksum(); - uint16_t received_checksum = *(uint16_t *)(buffer_decoded + data_size - 2); - if (checksum == received_checksum) { - // Packet checksum is OK, process it - switch (buffer_decoded[0]) { - case PACKET_ID_LL_STATUS: - if (data_size == sizeof(struct ll_status)) { - handleLowLevelStatus((struct ll_status *)buffer_decoded); - } else { - ROS_INFO_STREAM("Low Level Board sent a valid packet with the wrong size. Type was STATUS"); - } - break; - case PACKET_ID_LL_IMU: - if (data_size == sizeof(struct ll_imu)) { - handleLowLevelIMU((struct ll_imu *)buffer_decoded); - } else { - ROS_INFO_STREAM("Low Level Board sent a valid packet with the wrong size. Type was IMU"); - } - break; - case PACKET_ID_LL_UI_EVENT: - if (data_size == sizeof(struct ll_ui_event)) { - handleLowLevelUIEvent((struct ll_ui_event *)buffer_decoded); - } else { - ROS_INFO_STREAM("Low Level Board sent a valid packet with the wrong size. Type was UI_EVENT"); - } - break; - case PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ: - case PACKET_ID_LL_HIGH_LEVEL_CONFIG_RSP: - if (data_size == sizeof(struct ll_high_level_config)) { - handleLowLevelConfig((struct ll_high_level_config *)buffer_decoded); - } else { - ROS_INFO_STREAM("Low Level Board sent a valid packet with the wrong size. Type was CONFIG_* (0x" - << std::hex << buffer_decoded[0] << ")"); - } - break; - default: ROS_INFO_STREAM("Got unknown packet from Low Level Board"); break; - } - } else { - ROS_INFO_STREAM("Got invalid checksum from Low Level Board"); - } - } - - read = 0; - } else { - read += bytes_read; - } - } - } + // ros::Subscriber high_level_status_sub = n.subscribe("/mower_logic/current_state", 0, highLevelStatusReceived); + // ros::Timer publish_timer = n.createTimer(ros::Duration(0.02), publishActuatorsTimerTask); - spinner.stop(); + ctx = xbot::serviceif::Start(); - if (mow_xesc_interface) { - mow_xesc_interface->setDutyCycle(0.0); - mow_xesc_interface->stop(); - } - left_xesc_interface->setDutyCycle(0.0); - right_xesc_interface->setDutyCycle(0.0); - left_xesc_interface->stop(); - right_xesc_interface->stop(); + emergency_service = std::make_unique(1, ctx, emergency_pub); + diff_drive_service = + std::make_unique(2, ctx, actual_twist_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); + + emergency_service->Start(); + diff_drive_service->Start(); + mower_service->Start(); + imu_service->Start(); - if (mow_xesc_interface) { - delete mow_xesc_interface; + while (ctx.io->OK() && ros::ok()) { + ros::spinOnce(); } - delete left_xesc_interface; - delete right_xesc_interface; return 0; } diff --git a/src/mower_msgs/msg/Status.msg b/src/mower_msgs/msg/Status.msg index ada325e4..1f7bc1bd 100644 --- a/src/mower_msgs/msg/Status.msg +++ b/src/mower_msgs/msg/Status.msg @@ -20,6 +20,7 @@ float32 charge_current bool mow_enabled # ESC stuff -ESCStatus left_esc_status -ESCStatus right_esc_status -ESCStatus mow_esc_status +float32 mower_esc_temperature +float32 mower_esc_current +float32 mower_motor_temperature +float32 mower_motor_rpm From 60696db01d6ea176d62c452bb83f995b08a9eb79 Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Mon, 9 Sep 2024 19:59:17 +0200 Subject: [PATCH 06/29] wip --- services/diff_drive_service.json | 6 +- services/docking_sensor_service.json | 24 +++++++ services/emergency_service.json | 6 +- services/gps_service.json | 65 +++++++++++++++++++ services/imu_service.json | 6 +- services/lidar_service.json | 48 ++++++++++++++ services/mower_service.json | 41 +++--------- services/mower_ui_service.json | 6 +- services/power_service.json | 39 +++++++++++ src/lib/xbot_driver_gps | 2 +- src/mower_comms_v2/CMakeLists.txt | 9 +++ .../src/DiffDriveServiceInterface.cpp | 34 +++++----- .../src/DiffDriveServiceInterface.h | 20 +++--- .../src/DockingSensorServiceInterface.cpp | 26 ++++++++ .../src/DockingSensorServiceInterface.h | 33 ++++++++++ .../src/EmergencyServiceInterface.cpp | 10 ++- .../src/EmergencyServiceInterface.h | 1 + src/mower_comms_v2/src/ImuServiceInterface.h | 2 +- .../src/LidarServiceInterface.cpp | 46 +++++++++++++ .../src/LidarServiceInterface.h | 37 +++++++++++ .../src/MowerServiceInterface.cpp | 20 ++---- .../src/MowerServiceInterface.h | 14 ++-- .../src/PowerServiceInterface.cpp | 31 +++++++++ .../src/PowerServiceInterface.h | 35 ++++++++++ src/mower_comms_v2/src/mower_comms.cpp | 54 +++++++++++---- src/mower_msgs/CMakeLists.txt | 6 +- src/mower_msgs/msg/DockingSensor.msg | 4 ++ src/mower_msgs/msg/Power.msg | 7 ++ src/open_mower/launch/include/_teleop.launch | 2 +- 29 files changed, 521 insertions(+), 113 deletions(-) create mode 100644 services/docking_sensor_service.json create mode 100644 services/gps_service.json create mode 100644 services/lidar_service.json create mode 100644 services/power_service.json create mode 100644 src/mower_comms_v2/src/DockingSensorServiceInterface.cpp create mode 100644 src/mower_comms_v2/src/DockingSensorServiceInterface.h create mode 100644 src/mower_comms_v2/src/LidarServiceInterface.cpp create mode 100644 src/mower_comms_v2/src/LidarServiceInterface.h create mode 100644 src/mower_comms_v2/src/PowerServiceInterface.cpp create mode 100644 src/mower_comms_v2/src/PowerServiceInterface.h create mode 100644 src/mower_msgs/msg/DockingSensor.msg create mode 100644 src/mower_msgs/msg/Power.msg diff --git a/services/diff_drive_service.json b/services/diff_drive_service.json index 13351b6d..2082b1f2 100644 --- a/services/diff_drive_service.json +++ b/services/diff_drive_service.json @@ -1,6 +1,4 @@ { - "type": "DiffDriveService", - "version": 1, "inputs": [ { "id": 0, @@ -46,5 +44,7 @@ "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 index b412d215..38d68d01 100644 --- a/services/emergency_service.json +++ b/services/emergency_service.json @@ -1,6 +1,4 @@ { - "type": "EmergencyService", - "version": 1, "inputs": [ { "id": 0, @@ -25,5 +23,7 @@ "type": "char[255]" } ], - "registers": [] + "registers": [], + "type": "EmergencyService", + "version": 1 } diff --git a/services/gps_service.json b/services/gps_service.json new file mode 100644 index 00000000..1ce84590 --- /dev/null +++ b/services/gps_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": "MotionVectorENU", + "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": "GpsService", + "version": 1 +} diff --git a/services/imu_service.json b/services/imu_service.json index 2aff2c51..5e26fe07 100644 --- a/services/imu_service.json +++ b/services/imu_service.json @@ -1,6 +1,4 @@ { - "type": "IMUService", - "version": 1, "inputs": [], "outputs": [ { @@ -9,5 +7,7 @@ "type": "double[9]" } ], - "registers": [] + "registers": [], + "type": "IMUService", + "version": 1 } diff --git a/services/lidar_service.json b/services/lidar_service.json new file mode 100644 index 00000000..e3c42926 --- /dev/null +++ b/services/lidar_service.json @@ -0,0 +1,48 @@ +{ + "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]" + } + ], + "registers": [] +} diff --git a/services/mower_service.json b/services/mower_service.json index 0f92bd55..187b7354 100644 --- a/services/mower_service.json +++ b/services/mower_service.json @@ -1,6 +1,4 @@ { - "type": "MowerService", - "version": 1, "inputs": [ { "id": 0, @@ -16,59 +14,36 @@ }, { "id": 1, - "name": "Raspberry Pi Power", - "type": "uint8_t" - }, - { - "id": 2, - "name": "GPS Power", - "type": "uint8_t" - }, - { - "id": 3, - "name": "ESC Power", - "type": "uint8_t" - }, - { - "id": 4, "name": "Rain Detected", "type": "uint8_t" }, { - "id": 5, - "name": "Charge Voltage", - "type": "float" - }, - { - "id": 6, - "name": "Battery Voltage", - "type": "float" - }, - { - "id": 7, + "id": 2, "name": "Mower Running", "type": "uint8_t" }, { - "id": 8, + "id": 3, "name": "Mower ESC Temperature", "type": "float" }, { - "id": 9, + "id": 4, "name": "Mower Motor Temperature", "type": "float" }, { - "id": 10, + "id": 5, "name": "Mower Motor Current", "type": "float" }, { - "id": 11, + "id": 6, "name": "Mower Motor RPM", "type": "float" } ], - "registers": [] + "registers": [], + "type": "MowerService", + "version": 1 } diff --git a/services/mower_ui_service.json b/services/mower_ui_service.json index 5acf4d19..745b7bfb 100644 --- a/services/mower_ui_service.json +++ b/services/mower_ui_service.json @@ -1,6 +1,4 @@ { - "type": "MowerUiService", - "version": 1, "inputs": [ { "id": 0, @@ -56,5 +54,7 @@ "name": "Wheel Distance", "type": "double" } - ] + ], + "type": "MowerUiService", + "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_driver_gps b/src/lib/xbot_driver_gps index 99a53c42..76fe27d6 160000 --- a/src/lib/xbot_driver_gps +++ b/src/lib/xbot_driver_gps @@ -1 +1 @@ -Subproject commit 99a53c42f2f86595aff7f34bdbfee086ae61cff1 +Subproject commit 76fe27d6474789693525ac53027f4ee32be1eaed diff --git a/src/mower_comms_v2/CMakeLists.txt b/src/mower_comms_v2/CMakeLists.txt index 3d03f33c..bad0e302 100644 --- a/src/mower_comms_v2/CMakeLists.txt +++ b/src/mower_comms_v2/CMakeLists.txt @@ -171,12 +171,21 @@ add_executable(mower_comms_v2 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 ) 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) add_dependencies(mower_comms_v2 ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) diff --git a/src/mower_comms_v2/src/DiffDriveServiceInterface.cpp b/src/mower_comms_v2/src/DiffDriveServiceInterface.cpp index c3edd2a7..e8d65b1b 100644 --- a/src/mower_comms_v2/src/DiffDriveServiceInterface.cpp +++ b/src/mower_comms_v2/src/DiffDriveServiceInterface.cpp @@ -16,6 +16,7 @@ bool DiffDriveServiceInterface::OnConfigurationRequested(const std::string& uid) } 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; @@ -25,19 +26,11 @@ void DiffDriveServiceInterface::SendTwist(const geometry_msgs::TwistConstPtr& ms data[5] = msg->angular.z; SendControlTwist(data, 6); } -void DiffDriveServiceInterface::GetEscInfo(bool& left_error, float& left_esc_temperature, float& left_esc_current, - bool& right_error, float& right_esc_temperature, float& right_esc_current) { - std::unique_lock lk{state_mutex_}; - left_error = left_esc_state_.error; - left_esc_temperature = left_esc_state_.temperature; - left_esc_current = left_esc_state_.current; - right_error = right_esc_state_.error; - right_esc_temperature = right_esc_state_.temperature; - right_esc_current = right_esc_state_.current; -} + 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(); @@ -58,7 +51,7 @@ void DiffDriveServiceInterface::OnLeftESCCurrentChanged(const float& new_value) } void DiffDriveServiceInterface::OnRightESCTemperatureChanged(const float& new_value) { std::unique_lock lk{state_mutex_}; - right_esc_state_.temperature = new_value; + right_esc_state_.temperature_pcb = new_value; } void DiffDriveServiceInterface::OnRightESCCurrentChanged(const float& new_value) { std::unique_lock lk{state_mutex_}; @@ -66,19 +59,24 @@ void DiffDriveServiceInterface::OnRightESCCurrentChanged(const float& new_value) } void DiffDriveServiceInterface::OnLeftESCTemperatureChanged(const float& new_value) { std::unique_lock lk{state_mutex_}; - left_esc_state_.temperature = new_value; + left_esc_state_.temperature_pcb = new_value; } void DiffDriveServiceInterface::OnServiceConnected(const std::string& uid) { std::unique_lock lk{state_mutex_}; - left_esc_state_.error = false; - right_esc_state_.error = false; + left_esc_state_.status = mower_msgs::ESCStatus::ESC_STATUS_OK; + right_esc_state_.status = mower_msgs::ESCStatus::ESC_STATUS_OK; } void DiffDriveServiceInterface::OnTransactionStart(uint64_t timestamp) { } -void DiffDriveServiceInterface::OnTransactionEnd() { -} + void DiffDriveServiceInterface::OnServiceDisconnected(const std::string& uid) { std::unique_lock lk{state_mutex_}; - left_esc_state_.error = true; - right_esc_state_.error = true; + 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 index 61997116..183cff62 100644 --- a/src/mower_comms_v2/src/DiffDriveServiceInterface.h +++ b/src/mower_comms_v2/src/DiffDriveServiceInterface.h @@ -6,6 +6,8 @@ #define DIFFDRIVESERVICEINTERFACE_H #include +#include +#include #include #include @@ -13,9 +15,14 @@ class DiffDriveServiceInterface : public DiffDriveServiceInterfaceBase { public: DiffDriveServiceInterface(uint16_t service_id, const xbot::serviceif::Context& ctx, - const ros::Publisher& actual_twist_publisher, double ticks_per_meter, double wheel_distance) + 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) { } @@ -27,8 +34,6 @@ class DiffDriveServiceInterface : public DiffDriveServiceInterfaceBase { * @param msg The ROS message */ void SendTwist(const geometry_msgs::TwistConstPtr& msg); - void GetEscInfo(bool& left_error, float& left_esc_temperature, float& left_esc_current, bool& right_error, - float& right_esc_temperature, float& right_esc_current); protected: /** @@ -55,15 +60,14 @@ class DiffDriveServiceInterface : public DiffDriveServiceInterfaceBase { 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 - struct ESCState { - bool error = true; - float temperature = 0.0; - float current = 0.0; - } left_esc_state_{}, right_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..f1dbc389 --- /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(const std::string& uid) { + 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(const std::string& uid) { +} +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..82482f3e --- /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(const std::string& uid) override; + + protected: + void OnDetectedSensorsLeftChanged(const uint8_t& new_value) override; + void OnDetectedSensorsRightChanged(const uint8_t& new_value) override; + + private: + void OnServiceConnected(const std::string& uid) 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 index cbe2fc0e..fe87b7c0 100644 --- a/src/mower_comms_v2/src/EmergencyServiceInterface.cpp +++ b/src/mower_comms_v2/src/EmergencyServiceInterface.cpp @@ -23,6 +23,9 @@ bool EmergencyServiceInterface::SetEmergency(bool new_value) { PublishEmergencyState(); return true; } +void EmergencyServiceInterface::Heartbeat() { + SendSetEmergency(latched_emergency_); +} bool EmergencyServiceInterface::OnConfigurationRequested(const std::string& uid) { // No config needed @@ -35,8 +38,11 @@ void EmergencyServiceInterface::OnEmergencyActiveChanged(const uint8_t& new_valu void EmergencyServiceInterface::OnEmergencyLatchChanged(const uint8_t& new_value) { std::unique_lock lk{state_mutex_}; - if (!new_value && !active_high_level_emergency_) { - // Only clear latch, if we don't have a high level emergency going on + 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; } } diff --git a/src/mower_comms_v2/src/EmergencyServiceInterface.h b/src/mower_comms_v2/src/EmergencyServiceInterface.h index b00b3ea4..678326e5 100644 --- a/src/mower_comms_v2/src/EmergencyServiceInterface.h +++ b/src/mower_comms_v2/src/EmergencyServiceInterface.h @@ -18,6 +18,7 @@ class EmergencyServiceInterface : public EmergencyServiceInterfaceBase { } bool SetEmergency(bool new_value); + void Heartbeat(); protected: bool OnConfigurationRequested(const std::string& uid) override; diff --git a/src/mower_comms_v2/src/ImuServiceInterface.h b/src/mower_comms_v2/src/ImuServiceInterface.h index 7ea1982d..b716e681 100644 --- a/src/mower_comms_v2/src/ImuServiceInterface.h +++ b/src/mower_comms_v2/src/ImuServiceInterface.h @@ -8,7 +8,7 @@ #include #include -#include +#include class ImuServiceInterface : public IMUServiceInterfaceBase { public: diff --git a/src/mower_comms_v2/src/LidarServiceInterface.cpp b/src/mower_comms_v2/src/LidarServiceInterface.cpp new file mode 100644 index 00000000..7bad5ab9 --- /dev/null +++ b/src/mower_comms_v2/src/LidarServiceInterface.cpp @@ -0,0 +1,46 @@ +// +// Created by clemens on 28.08.24. +// + +#include "LidarServiceInterface.h" +bool LidarServiceInterface::OnConfigurationRequested(const std::string& uid) { + return true; +} +void LidarServiceInterface::OnAngleMinRadChanged(const double& new_value) { + laser_scan_msg_.angle_min = new_value; +} +void LidarServiceInterface::OnAngleMaxRadChanged(const double& new_value) { + laser_scan_msg_.angle_max = new_value; +} +void LidarServiceInterface::OnTimeIncrementSecondsChanged(const float& new_value) { + laser_scan_msg_.time_increment = new_value; +} +void LidarServiceInterface::OnScanTimeSecondsChanged(const float& new_value) { + laser_scan_msg_.scan_time = new_value; +} +void LidarServiceInterface::OnRangeMinMChanged(const float& new_value) { + laser_scan_msg_.range_min = new_value; +} +void LidarServiceInterface::OnRangeMaxMChanged(const float& new_value) { + laser_scan_msg_.range_max = new_value; +} +void LidarServiceInterface::OnRangesChanged(const float* new_value, uint32_t length) { + laser_scan_msg_.ranges.clear(); + laser_scan_msg_.ranges.insert(laser_scan_msg_.ranges.end(), new_value, new_value + length); +} +void LidarServiceInterface::OnIntensitiesChanged(const float* new_value, uint32_t length) { + laser_scan_msg_.intensities.clear(); + laser_scan_msg_.intensities.insert(laser_scan_msg_.intensities.end(), new_value, new_value + length); +} +void LidarServiceInterface::OnTransactionEnd() { + laser_scan_msg_.header.frame_id = "lidar"; + laser_scan_msg_.header.seq++; + laser_scan_msg_.header.stamp = ros::Time::now(); + laser_scan_msg_.range_min = 0.0; + laser_scan_msg_.range_max = 20.0; + laser_scan_msg_.angle_increment = + fmod(laser_scan_msg_.angle_max - laser_scan_msg_.angle_min + 2.0 * M_PI, 2.0 * M_PI) / + laser_scan_msg_.ranges.size(); + + lidar_publisher_.publish(laser_scan_msg_); +} diff --git a/src/mower_comms_v2/src/LidarServiceInterface.h b/src/mower_comms_v2/src/LidarServiceInterface.h new file mode 100644 index 00000000..8f0bcba9 --- /dev/null +++ b/src/mower_comms_v2/src/LidarServiceInterface.h @@ -0,0 +1,37 @@ +// +// 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(const std::string& uid) 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; + + private: + void OnTransactionEnd() override; + + const ros::Publisher& lidar_publisher_; + 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 index 85888e5c..a524df56 100644 --- a/src/mower_comms_v2/src/MowerServiceInterface.cpp +++ b/src/mower_comms_v2/src/MowerServiceInterface.cpp @@ -8,27 +8,15 @@ bool MowerServiceInterface::OnConfigurationRequested(const std::string& uid) { // 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::OnRaspberryPiPowerChanged(const uint8_t& new_value) { - status_msg_.raspberry_pi_power = new_value; -} -void MowerServiceInterface::OnGPSPowerChanged(const uint8_t& new_value) { - status_msg_.gps_power = new_value; -} -void MowerServiceInterface::OnESCPowerChanged(const uint8_t& new_value) { - status_msg_.esc_power = new_value; -} void MowerServiceInterface::OnRainDetectedChanged(const uint8_t& new_value) { status_msg_.rain_detected = new_value; } -void MowerServiceInterface::OnChargeVoltageChanged(const float& new_value) { - status_msg_.v_charge = new_value; -} -void MowerServiceInterface::OnBatteryVoltageChanged(const float& new_value) { - status_msg_.v_battery = new_value; -} void MowerServiceInterface::OnMowerRunningChanged(const uint8_t& new_value) { status_msg_.mow_enabled = new_value; } @@ -51,7 +39,7 @@ void MowerServiceInterface::OnTransactionStart(uint64_t timestamp) { status_msg_.stamp = ros::Time::now(); } void MowerServiceInterface::OnTransactionEnd() { - status_poblisher_.publish(status_msg_); + status_publisher_.publish(status_msg_); } void MowerServiceInterface::OnServiceDisconnected(const std::string& uid) { } diff --git a/src/mower_comms_v2/src/MowerServiceInterface.h b/src/mower_comms_v2/src/MowerServiceInterface.h index 63f6e4cc..040d0f28 100644 --- a/src/mower_comms_v2/src/MowerServiceInterface.h +++ b/src/mower_comms_v2/src/MowerServiceInterface.h @@ -13,20 +13,16 @@ class MowerServiceInterface : public MowerServiceInterfaceBase { public: MowerServiceInterface(uint16_t service_id, const xbot::serviceif::Context& ctx, - const ros::Publisher& status_poblisher) - : MowerServiceInterfaceBase(service_id, ctx), status_poblisher_(status_poblisher) { + const ros::Publisher& status_publisher) + : MowerServiceInterfaceBase(service_id, ctx), status_publisher_(status_publisher) { } bool OnConfigurationRequested(const std::string& uid) override; + void SetMowerEnabled(bool enabled); protected: void OnMowerStatusChanged(const uint8_t& new_value) override; - void OnRaspberryPiPowerChanged(const uint8_t& new_value) override; - void OnGPSPowerChanged(const uint8_t& new_value) override; - void OnESCPowerChanged(const uint8_t& new_value) override; void OnRainDetectedChanged(const uint8_t& new_value) override; - void OnChargeVoltageChanged(const float& new_value) override; - void OnBatteryVoltageChanged(const float& 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; @@ -38,8 +34,10 @@ class MowerServiceInterface : public MowerServiceInterfaceBase { void OnTransactionStart(uint64_t timestamp) override; void OnTransactionEnd() override; void OnServiceDisconnected(const std::string& uid) override; + + private: mower_msgs::Status status_msg_{}; - const ros::Publisher& status_poblisher_; + 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..d009cb5f --- /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(const std::string& uid) { + 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..6c4b147a --- /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(const std::string& uid) 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 index b083294e..047b39d0 100644 --- a/src/mower_comms_v2/src/mower_comms.cpp +++ b/src/mower_comms_v2/src/mower_comms.cpp @@ -17,22 +17,32 @@ // // #include +#include #include #include #include +#include #include #include #include "DiffDriveServiceInterface.h" +#include "DockingSensorServiceInterface.h" #include "EmergencyServiceInterface.h" #include "ImuServiceInterface.h" +#include "LidarServiceInterface.h" #include "MowerServiceInterface.h" +#include "PowerServiceInterface.h" ros::Publisher status_pub; +ros::Publisher power_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; @@ -40,6 +50,9 @@ 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; xbot::serviceif::Context ctx{}; @@ -57,6 +70,15 @@ void velReceived(const geometry_msgs::Twist::ConstPtr &msg) { 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"); @@ -75,29 +97,39 @@ int main(int argc, char **argv) { ROS_INFO_STREAM("Wheel ticks [1/m]: " << wheel_ticks_per_m); ROS_INFO_STREAM("Wheel distance [m]: " << wheel_distance_m); - status_pub = n.advertise("mower/status", 1); - emergency_pub = n.advertise("mower/emergency", 1); - actual_twist_pub = n.advertise("mower/actual_twist", 1); - - sensor_imu_pub = n.advertise("imu/data_raw", 1); - // ros::ServiceServer mow_service = n.advertiseService("mower_service/mow_enabled", setMowEnabled); - ros::ServiceServer ros_emergency_service = n.advertiseService("mower_service/emergency", setEmergencyStop); - ros::Subscriber cmd_vel_sub = n.subscribe("cmd_vel", 0, velReceived, ros::TransportHints().tcpNoDelay(true)); + 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/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); + 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.02), publishActuatorsTimerTask); + ros::Timer publish_timer = n.createTimer(ros::Duration(0.5), sendEmergencyHeartbeatTimerTask); ctx = xbot::serviceif::Start(); emergency_service = std::make_unique(1, ctx, emergency_pub); - diff_drive_service = - std::make_unique(2, ctx, actual_twist_pub, wheel_ticks_per_m, wheel_distance_m); + 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); + 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(); while (ctx.io->OK() && ros::ok()) { ros::spinOnce(); diff --git a/src/mower_msgs/CMakeLists.txt b/src/mower_msgs/CMakeLists.txt index 3346b105..f495aad2 100644 --- a/src/mower_msgs/CMakeLists.txt +++ b/src/mower_msgs/CMakeLists.txt @@ -13,7 +13,9 @@ add_message_files( ESCStatus.msg HighLevelStatus.msg Perimeter.msg - Emergency.msg + Emergency.msg + Power.msg + DockingSensor.msg ) add_service_files( @@ -22,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/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/open_mower/launch/include/_teleop.launch b/src/open_mower/launch/include/_teleop.launch index 1439ac8b..c0ee22a3 100644 --- a/src/open_mower/launch/include/_teleop.launch +++ b/src/open_mower/launch/include/_teleop.launch @@ -6,7 +6,7 @@ - + From beb3ebd2e474bd04e730519c5b6038b4848e2d40 Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Mon, 9 Sep 2024 20:40:27 +0200 Subject: [PATCH 07/29] removed dependencies --- src/mower_comms_v2/CMakeLists.txt | 1 - src/mower_comms_v2/package.xml | 54 ++++++++++++++----------------- 2 files changed, 24 insertions(+), 31 deletions(-) diff --git a/src/mower_comms_v2/CMakeLists.txt b/src/mower_comms_v2/CMakeLists.txt index bad0e302..5ac509e5 100644 --- a/src/mower_comms_v2/CMakeLists.txt +++ b/src/mower_comms_v2/CMakeLists.txt @@ -11,7 +11,6 @@ find_package(catkin REQUIRED COMPONENTS mower_msgs sensor_msgs roscpp - serial xesc_driver xesc_msgs xesc_interface diff --git a/src/mower_comms_v2/package.xml b/src/mower_comms_v2/package.xml index 7a95e360..cfd53e7e 100644 --- a/src/mower_comms_v2/package.xml +++ b/src/mower_comms_v2/package.xml @@ -1,38 +1,32 @@ - mower_comms_v2 - 0.0.0 - The mower_comms_v2 package - Clemens Elflein - CC BY-NC-SA 4.0 + mower_comms_v2 + 0.0.0 + The mower_comms_v2 package + Clemens Elflein + CC BY-NC-SA 4.0 - https://github.com/ClemensElflein/OpenMower + https://github.com/ClemensElflein/OpenMower - catkin - xbot_framework - xbot_msgs - mower_msgs - sensor_msgs - roscpp - serial - xesc_driver - ublox_msgs - xbot_msgs - mower_msgs - roscpp - serial - xesc_driver - xbot_msgs - mower_msgs - roscpp - serial - xesc_driver - xesc_msgs + catkin + xbot_framework + xbot_msgs + mower_msgs + sensor_msgs + roscpp + ublox_msgs + xbot_msgs + mower_msgs + roscpp + xbot_msgs + mower_msgs + roscpp + xesc_msgs - - - + + + - + From db9d770ee361d3cf44bec345ee783dfe6a4c20b9 Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Tue, 10 Sep 2024 01:42:17 +0200 Subject: [PATCH 08/29] wip --- services/imu_service.json | 2 +- src/mower_comms_v1/CMakeLists.txt | 20 ++++++++++---------- src/mower_comms_v2/CMakeLists.txt | 5 +---- src/mower_comms_v2/src/ImuServiceInterface.h | 4 ++-- 4 files changed, 14 insertions(+), 17 deletions(-) diff --git a/services/imu_service.json b/services/imu_service.json index 5e26fe07..384378d8 100644 --- a/services/imu_service.json +++ b/services/imu_service.json @@ -8,6 +8,6 @@ } ], "registers": [], - "type": "IMUService", + "type": "ImuService", "version": 1 } diff --git a/src/mower_comms_v1/CMakeLists.txt b/src/mower_comms_v1/CMakeLists.txt index 76b8b22b..79431ba3 100644 --- a/src/mower_comms_v1/CMakeLists.txt +++ b/src/mower_comms_v1/CMakeLists.txt @@ -8,10 +8,10 @@ project(mower_comms_v1) ## 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_msgs sensor_msgs - roscpp - serial + roscpp + serial xesc_driver xesc_msgs xesc_interface @@ -115,10 +115,10 @@ add_service(DiffDriveService ${CMAKE_CURRENT_SOURCE_DIR}/../../services/diff_dri ## 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_v1 -# 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 ) ########### @@ -128,8 +128,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 @@ -166,7 +166,7 @@ add_executable(mower_comms_v1 src/mower_comms.cpp src/COBS.h src/ll_datatypes.h - ) +) add_dependencies(mower_comms_v1 ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(mower_comms_v1 ${catkin_LIBRARIES} diff --git a/src/mower_comms_v2/CMakeLists.txt b/src/mower_comms_v2/CMakeLists.txt index 5ac509e5..53b1575c 100644 --- a/src/mower_comms_v2/CMakeLists.txt +++ b/src/mower_comms_v2/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.16 FATAL_ERROR) project(mower_comms_v2) ## Compile as C++11, supported in ROS Kinetic and newer @@ -11,11 +11,8 @@ find_package(catkin REQUIRED COMPONENTS mower_msgs sensor_msgs roscpp - xesc_driver xesc_msgs - xesc_interface xbot_msgs - # xbot_framework ) find_package( diff --git a/src/mower_comms_v2/src/ImuServiceInterface.h b/src/mower_comms_v2/src/ImuServiceInterface.h index b716e681..022afdf2 100644 --- a/src/mower_comms_v2/src/ImuServiceInterface.h +++ b/src/mower_comms_v2/src/ImuServiceInterface.h @@ -10,10 +10,10 @@ #include -class ImuServiceInterface : public IMUServiceInterfaceBase { +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) { + : ImuServiceInterfaceBase(service_id, ctx), imu_publisher_(imu_publisher) { } bool OnConfigurationRequested(const std::string& uid) override; From a9d6cc62d2ecf8ce9a47220254d8ca3a8c50c4ca Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Tue, 10 Sep 2024 02:27:40 +0200 Subject: [PATCH 09/29] allow specifying bind_ip --- src/lib/xbot_framework | 2 +- src/mower_comms_v2/src/mower_comms.cpp | 5 ++++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/lib/xbot_framework b/src/lib/xbot_framework index c8d23684..96c5c463 160000 --- a/src/lib/xbot_framework +++ b/src/lib/xbot_framework @@ -1 +1 @@ -Subproject commit c8d2368471664ff6ce6563ea328dc886848c9476 +Subproject commit 96c5c463a1cf182d8676b1bc3ae423f50842b17c diff --git a/src/mower_comms_v2/src/mower_comms.cpp b/src/mower_comms_v2/src/mower_comms.cpp index 047b39d0..f4a470c7 100644 --- a/src/mower_comms_v2/src/mower_comms.cpp +++ b/src/mower_comms_v2/src/mower_comms.cpp @@ -91,9 +91,12 @@ int main(int argc, char **argv) { 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); @@ -112,7 +115,7 @@ int main(int argc, char **argv) { // 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(); + ctx = xbot::serviceif::Start(true, bind_ip); emergency_service = std::make_unique(1, ctx, emergency_pub); diff_drive_service = std::make_unique( From fd055727bc279afad7c90217eddae041e958a37f Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Tue, 10 Sep 2024 02:46:45 +0200 Subject: [PATCH 10/29] wip --- src/lib/xbot_framework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/xbot_framework b/src/lib/xbot_framework index 96c5c463..b10b1272 160000 --- a/src/lib/xbot_framework +++ b/src/lib/xbot_framework @@ -1 +1 @@ -Subproject commit 96c5c463a1cf182d8676b1bc3ae423f50842b17c +Subproject commit b10b1272d3d38db6c4af6d896e4d350abcf846a0 From f96e951fbb9532c557ff0ae896296e75734affb2 Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Tue, 10 Sep 2024 14:15:48 +0200 Subject: [PATCH 11/29] update xbot_framework --- src/lib/xbot_framework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/xbot_framework b/src/lib/xbot_framework index b10b1272..dd87dc55 160000 --- a/src/lib/xbot_framework +++ b/src/lib/xbot_framework @@ -1 +1 @@ -Subproject commit b10b1272d3d38db6c4af6d896e4d350abcf846a0 +Subproject commit dd87dc55a1a81c95b596c22ba898453408994139 From fe5d4fa9c90efa431e8b52d45211655b3d0331a2 Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Tue, 10 Sep 2024 14:49:51 +0200 Subject: [PATCH 12/29] add v2 comms to launch file --- src/open_mower/launch/include/_comms.launch | 36 +++++++++++++++------ 1 file changed, 26 insertions(+), 10 deletions(-) diff --git a/src/open_mower/launch/include/_comms.launch b/src/open_mower/launch/include/_comms.launch index 178bec7c..9f36fa32 100644 --- a/src/open_mower/launch/include/_comms.launch +++ b/src/open_mower/launch/include/_comms.launch @@ -4,17 +4,33 @@ --> + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - From 46b997ec26691874eccab441663ee50abb358aa6 Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Tue, 10 Sep 2024 15:12:26 +0200 Subject: [PATCH 13/29] update xbot_framework --- src/lib/xbot_framework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/xbot_framework b/src/lib/xbot_framework index dd87dc55..b643e049 160000 --- a/src/lib/xbot_framework +++ b/src/lib/xbot_framework @@ -1 +1 @@ -Subproject commit dd87dc55a1a81c95b596c22ba898453408994139 +Subproject commit b643e049e385a93714c7042548c1d0e160f0b910 From e0c717a7188434b54cede175f42f6ed38a1a2771 Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Tue, 10 Sep 2024 15:50:07 +0200 Subject: [PATCH 14/29] added wheel ticks to status messages --- services/diff_drive_service.json | 5 +++++ src/mower_comms_v2/src/DiffDriveServiceInterface.cpp | 5 +++++ src/mower_comms_v2/src/DiffDriveServiceInterface.h | 1 + src/mower_comms_v2/src/mower_comms.cpp | 2 +- 4 files changed, 12 insertions(+), 1 deletion(-) diff --git a/services/diff_drive_service.json b/services/diff_drive_service.json index 2082b1f2..aeb5f7b5 100644 --- a/services/diff_drive_service.json +++ b/services/diff_drive_service.json @@ -31,6 +31,11 @@ "id": 4, "name": "Right ESC Current", "type": "float" + }, + { + "id": 5, + "name": "Wheel Ticks", + "type": "uint32_t[2]" } ], "registers": [ diff --git a/src/mower_comms_v2/src/DiffDriveServiceInterface.cpp b/src/mower_comms_v2/src/DiffDriveServiceInterface.cpp index e8d65b1b..0bab6bf8 100644 --- a/src/mower_comms_v2/src/DiffDriveServiceInterface.cpp +++ b/src/mower_comms_v2/src/DiffDriveServiceInterface.cpp @@ -57,6 +57,11 @@ 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; diff --git a/src/mower_comms_v2/src/DiffDriveServiceInterface.h b/src/mower_comms_v2/src/DiffDriveServiceInterface.h index 183cff62..d7ae1175 100644 --- a/src/mower_comms_v2/src/DiffDriveServiceInterface.h +++ b/src/mower_comms_v2/src/DiffDriveServiceInterface.h @@ -46,6 +46,7 @@ class DiffDriveServiceInterface : public DiffDriveServiceInterfaceBase { 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; private: void OnServiceConnected(const std::string& uid) override; diff --git a/src/mower_comms_v2/src/mower_comms.cpp b/src/mower_comms_v2/src/mower_comms.cpp index f4a470c7..4b769673 100644 --- a/src/mower_comms_v2/src/mower_comms.cpp +++ b/src/mower_comms_v2/src/mower_comms.cpp @@ -104,7 +104,7 @@ int main(int argc, char **argv) { 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/measured_twist", 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); From fbe4f5eda1165a786d3eb4d21cbf7fb1e9de55da Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Tue, 10 Sep 2024 16:08:03 +0200 Subject: [PATCH 15/29] added params for Lubluelu robot --- .../Lubluelu/default_environment.sh | 12 ++++++++++++ 1 file changed, 12 insertions(+) create mode 100644 src/open_mower/params/hardware_specific/Lubluelu/default_environment.sh 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 From ff64e153d18c80c11e58490b2b68e65dcb5a0d06 Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Tue, 10 Sep 2024 16:10:37 +0200 Subject: [PATCH 16/29] fixed launch file --- src/open_mower/launch/include/_comms.launch | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/open_mower/launch/include/_comms.launch b/src/open_mower/launch/include/_comms.launch index 9f36fa32..509574b9 100644 --- a/src/open_mower/launch/include/_comms.launch +++ b/src/open_mower/launch/include/_comms.launch @@ -20,9 +20,7 @@ - - - + From ff4b7f051d6b77786b4fb4b196ecb078e41f1a6d Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Tue, 10 Sep 2024 23:13:12 +0200 Subject: [PATCH 17/29] wip lidar comms --- services/lidar_service.json | 5 + .../src/LidarServiceInterface.cpp | 94 +++++++++++++------ .../src/LidarServiceInterface.h | 41 +++++--- 3 files changed, 100 insertions(+), 40 deletions(-) diff --git a/services/lidar_service.json b/services/lidar_service.json index e3c42926..542abd8a 100644 --- a/services/lidar_service.json +++ b/services/lidar_service.json @@ -42,6 +42,11 @@ "id": 7, "name": "Intensities", "type": "float[100]" + }, + { + "id": 8, + "name": "NewScan", + "type": "uint8_t" } ], "registers": [] diff --git a/src/mower_comms_v2/src/LidarServiceInterface.cpp b/src/mower_comms_v2/src/LidarServiceInterface.cpp index 7bad5ab9..a711f76d 100644 --- a/src/mower_comms_v2/src/LidarServiceInterface.cpp +++ b/src/mower_comms_v2/src/LidarServiceInterface.cpp @@ -3,44 +3,80 @@ // #include "LidarServiceInterface.h" -bool LidarServiceInterface::OnConfigurationRequested(const std::string& uid) { + +bool LidarServiceInterface::OnConfigurationRequested(const std::string &uid) { return true; } -void LidarServiceInterface::OnAngleMinRadChanged(const double& new_value) { - laser_scan_msg_.angle_min = new_value; + +void LidarServiceInterface::OnAngleMinRadChanged(const double &new_value) { + angle_min = new_value; } -void LidarServiceInterface::OnAngleMaxRadChanged(const double& new_value) { - laser_scan_msg_.angle_max = new_value; + +void LidarServiceInterface::OnAngleMaxRadChanged(const double &new_value) { + angle_max = new_value; } -void LidarServiceInterface::OnTimeIncrementSecondsChanged(const float& new_value) { - laser_scan_msg_.time_increment = new_value; + +void LidarServiceInterface::OnTimeIncrementSecondsChanged(const float &new_value) { + time_increment = new_value; } -void LidarServiceInterface::OnScanTimeSecondsChanged(const float& new_value) { - laser_scan_msg_.scan_time = new_value; + +void LidarServiceInterface::OnScanTimeSecondsChanged(const float &new_value) { + scan_time = new_value; } -void LidarServiceInterface::OnRangeMinMChanged(const float& new_value) { - laser_scan_msg_.range_min = new_value; + +void LidarServiceInterface::OnRangeMinMChanged(const float &new_value) { + min_m = std::min(min_m, new_value); } -void LidarServiceInterface::OnRangeMaxMChanged(const float& new_value) { - laser_scan_msg_.range_max = 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) { - laser_scan_msg_.ranges.clear(); - laser_scan_msg_.ranges.insert(laser_scan_msg_.ranges.end(), new_value, new_value + length); + +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) { - laser_scan_msg_.intensities.clear(); - laser_scan_msg_.intensities.insert(laser_scan_msg_.intensities.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 = true; } + void LidarServiceInterface::OnTransactionEnd() { - laser_scan_msg_.header.frame_id = "lidar"; - laser_scan_msg_.header.seq++; - laser_scan_msg_.header.stamp = ros::Time::now(); - laser_scan_msg_.range_min = 0.0; - laser_scan_msg_.range_max = 20.0; - laser_scan_msg_.angle_increment = - fmod(laser_scan_msg_.angle_max - laser_scan_msg_.angle_min + 2.0 * M_PI, 2.0 * M_PI) / - laser_scan_msg_.ranges.size(); - - lidar_publisher_.publish(laser_scan_msg_); + if (new_scan) { + // Send the gathered data + if (laser_scan_msg_.ranges.size() > 0) { + laser_scan_msg_.header.frame_id = "base_link"; + laser_scan_msg_.header.seq++; + laser_scan_msg_.header.stamp = ros::Time::now(); + laser_scan_msg_.range_min = 0.0; + laser_scan_msg_.range_max = 20.0; + laser_scan_msg_.angle_increment = + fmod(laser_scan_msg_.angle_max - laser_scan_msg_.angle_min + 2.0 * M_PI, 2.0 * M_PI) / + laser_scan_msg_.ranges.size(); + + lidar_publisher_.publish(laser_scan_msg_); + } + new_scan = false; + + // Set the data from the last transaction as a start + laser_scan_msg_.angle_max = angle_max; + laser_scan_msg_.angle_min = angle_min; + laser_scan_msg_.ranges = ranges; + laser_scan_msg_.time_increment = time_increment; + laser_scan_msg_.scan_time = scan_time; + 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 = std::max(laser_scan_msg_.angle_max, angle_max); + laser_scan_msg_.angle_min = std::min(laser_scan_msg_.angle_min, angle_min); + laser_scan_msg_.ranges.clear(); + laser_scan_msg_.ranges.insert(laser_scan_msg_.ranges.end(), ranges.begin(), ranges.end()); + } } diff --git a/src/mower_comms_v2/src/LidarServiceInterface.h b/src/mower_comms_v2/src/LidarServiceInterface.h index 8f0bcba9..fcd9b189 100644 --- a/src/mower_comms_v2/src/LidarServiceInterface.h +++ b/src/mower_comms_v2/src/LidarServiceInterface.h @@ -12,25 +12,44 @@ class LidarServiceInterface : public LidarServiceInterfaceBase { public: - LidarServiceInterface(uint16_t service_id, const xbot::serviceif::Context& ctx, const ros::Publisher& lidar_publisher) + 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(const std::string& uid) override; + + bool OnConfigurationRequested(const std::string &uid) 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 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; - const ros::Publisher& lidar_publisher_; + private: + const ros::Publisher &lidar_publisher_; + std::vector 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; + sensor_msgs::LaserScan laser_scan_msg_{}; }; From 541e37f4b0dd61371aaeb7adbe440a20ed091aff Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Wed, 11 Sep 2024 02:18:54 +0200 Subject: [PATCH 18/29] fixed lidar --- src/lib/xbot_framework | 2 +- .../src/LidarServiceInterface.cpp | 50 +++++++++++++++---- .../src/LidarServiceInterface.h | 2 +- src/mower_comms_v2/src/mower_comms.cpp | 4 +- 4 files changed, 44 insertions(+), 14 deletions(-) diff --git a/src/lib/xbot_framework b/src/lib/xbot_framework index b643e049..a2c4330e 160000 --- a/src/lib/xbot_framework +++ b/src/lib/xbot_framework @@ -1 +1 @@ -Subproject commit b643e049e385a93714c7042548c1d0e160f0b910 +Subproject commit a2c4330e4a80dff64cd4d3bf4cb45f1ffa3c74cd diff --git a/src/mower_comms_v2/src/LidarServiceInterface.cpp b/src/mower_comms_v2/src/LidarServiceInterface.cpp index a711f76d..510d7c4b 100644 --- a/src/mower_comms_v2/src/LidarServiceInterface.cpp +++ b/src/mower_comms_v2/src/LidarServiceInterface.cpp @@ -4,6 +4,8 @@ #include "LidarServiceInterface.h" +#include "spdlog/spdlog.h" + bool LidarServiceInterface::OnConfigurationRequested(const std::string &uid) { return true; } @@ -41,30 +43,59 @@ void LidarServiceInterface::OnIntensitiesChanged(const float *new_value, uint32_ } void LidarServiceInterface::OnNewScanChanged(const uint8_t &new_value) { - new_scan = true; + 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); + } + } + + // SLAM node wants 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); + } + // Send the gathered data - if (laser_scan_msg_.ranges.size() > 0) { + if (!laser_scan_msg_.ranges.empty()) { laser_scan_msg_.header.frame_id = "base_link"; laser_scan_msg_.header.seq++; laser_scan_msg_.header.stamp = ros::Time::now(); laser_scan_msg_.range_min = 0.0; laser_scan_msg_.range_max = 20.0; laser_scan_msg_.angle_increment = - fmod(laser_scan_msg_.angle_max - laser_scan_msg_.angle_min + 2.0 * M_PI, 2.0 * M_PI) / + fmodf(laser_scan_msg_.angle_max - laser_scan_msg_.angle_min + 2.0 * M_PI, 2.0 * M_PI) / laser_scan_msg_.ranges.size(); + // our laser scanner is clockwise, so we need to flip some stuff. + // TODO: make this configurable + const auto tmp = laser_scan_msg_.angle_max; + laser_scan_msg_.angle_max = 360.0 - laser_scan_msg_.angle_min; + laser_scan_msg_.angle_min = 360.0 - tmp; + std::reverse(laser_scan_msg_.ranges.begin(), laser_scan_msg_.ranges.end()); + // END: flippening + lidar_publisher_.publish(laser_scan_msg_); } new_scan = false; // Set the data from the last transaction as a start - laser_scan_msg_.angle_max = angle_max; laser_scan_msg_.angle_min = angle_min; - laser_scan_msg_.ranges = ranges; + 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_.time_increment = time_increment; laser_scan_msg_.scan_time = scan_time; laser_scan_msg_.range_max = max_m; @@ -74,9 +105,10 @@ void LidarServiceInterface::OnTransactionEnd() { 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 = std::max(laser_scan_msg_.angle_max, angle_max); - laser_scan_msg_.angle_min = std::min(laser_scan_msg_.angle_min, angle_min); - laser_scan_msg_.ranges.clear(); - laser_scan_msg_.ranges.insert(laser_scan_msg_.ranges.end(), ranges.begin(), ranges.end()); + 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()); + } } } diff --git a/src/mower_comms_v2/src/LidarServiceInterface.h b/src/mower_comms_v2/src/LidarServiceInterface.h index fcd9b189..d6bb1f33 100644 --- a/src/mower_comms_v2/src/LidarServiceInterface.h +++ b/src/mower_comms_v2/src/LidarServiceInterface.h @@ -41,7 +41,7 @@ class LidarServiceInterface : public LidarServiceInterfaceBase { private: const ros::Publisher &lidar_publisher_; - std::vector ranges; + std::deque ranges; float max_m = 0; float min_m = 999; float scan_time = 0; diff --git a/src/mower_comms_v2/src/mower_comms.cpp b/src/mower_comms_v2/src/mower_comms.cpp index 4b769673..dbf96864 100644 --- a/src/mower_comms_v2/src/mower_comms.cpp +++ b/src/mower_comms_v2/src/mower_comms.cpp @@ -134,9 +134,7 @@ int main(int argc, char **argv) { power_service->Start(); docking_sensor_service->Start(); - while (ctx.io->OK() && ros::ok()) { - ros::spinOnce(); - } + ros::spin(); return 0; } From 3256fc5d37f7d00f0957498c13df2f6903bf559b Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Wed, 11 Sep 2024 14:59:20 +0200 Subject: [PATCH 19/29] wip lidar --- .../src/LidarServiceInterface.cpp | 70 ++++++++++++------- .../src/LidarServiceInterface.h | 5 +- 2 files changed, 47 insertions(+), 28 deletions(-) diff --git a/src/mower_comms_v2/src/LidarServiceInterface.cpp b/src/mower_comms_v2/src/LidarServiceInterface.cpp index 510d7c4b..7db19bb2 100644 --- a/src/mower_comms_v2/src/LidarServiceInterface.cpp +++ b/src/mower_comms_v2/src/LidarServiceInterface.cpp @@ -61,43 +61,55 @@ void LidarServiceInterface::OnTransactionEnd() { } } - // SLAM node wants 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); - } - // Send the gathered data + const auto now = ros::Time::now(); if (!laser_scan_msg_.ranges.empty()) { - laser_scan_msg_.header.frame_id = "base_link"; - laser_scan_msg_.header.seq++; - laser_scan_msg_.header.stamp = ros::Time::now(); - laser_scan_msg_.range_min = 0.0; - laser_scan_msg_.range_max = 20.0; - 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(); - - // our laser scanner is clockwise, so we need to flip some stuff. - // TODO: make this configurable - const auto tmp = laser_scan_msg_.angle_max; - laser_scan_msg_.angle_max = 360.0 - laser_scan_msg_.angle_min; - laser_scan_msg_.angle_min = 360.0 - tmp; - std::reverse(laser_scan_msg_.ranges.begin(), laser_scan_msg_.ranges.end()); - // END: flippening - - lidar_publisher_.publish(laser_scan_msg_); + 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(); + + 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(); + + // 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 + + 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_.time_increment = time_increment; - laser_scan_msg_.scan_time = scan_time; laser_scan_msg_.range_max = max_m; laser_scan_msg_.range_min = min_m; } else { @@ -112,3 +124,7 @@ void LidarServiceInterface::OnTransactionEnd() { } } } + +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 index d6bb1f33..ed4a74a7 100644 --- a/src/mower_comms_v2/src/LidarServiceInterface.h +++ b/src/mower_comms_v2/src/LidarServiceInterface.h @@ -39,7 +39,8 @@ class LidarServiceInterface : public LidarServiceInterfaceBase { private: void OnTransactionEnd() override; - private: + void OnTransactionStart(uint64_t timestamp) override; + const ros::Publisher &lidar_publisher_; std::deque ranges; float max_m = 0; @@ -49,6 +50,8 @@ class LidarServiceInterface : public LidarServiceInterfaceBase { 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_{}; }; From 5a5f02870d08a84d893c5da7775792d4cdde5f78 Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Wed, 13 Nov 2024 17:35:42 +0100 Subject: [PATCH 20/29] very much wip --- services/diff_drive_service.json | 18 +- src/lib/xbot_framework | 2 +- .../src/DiffDriveServiceInterface.cpp | 21 +- .../src/DiffDriveServiceInterface.h | 8 +- .../src/DockingSensorServiceInterface.cpp | 4 +- .../src/DockingSensorServiceInterface.h | 4 +- .../src/EmergencyServiceInterface.cpp | 6 +- .../src/EmergencyServiceInterface.h | 6 +- .../src/ImuServiceInterface.cpp | 6 +- src/mower_comms_v2/src/ImuServiceInterface.h | 6 +- .../src/LidarServiceInterface.cpp | 9 +- .../src/LidarServiceInterface.h | 61 +++--- .../src/MowerServiceInterface.cpp | 6 +- .../src/MowerServiceInterface.h | 6 +- .../src/PowerServiceInterface.cpp | 2 +- .../src/PowerServiceInterface.h | 2 +- src/mower_logic/CMakeLists.txt | 64 +++--- src/mower_logic/src/monitoring/monitoring.cpp | 83 ++++++-- .../mower_logic/behaviors/DockingBehavior.cpp | 14 +- .../mower_logic/behaviors/IdleBehavior.cpp | 12 +- .../behaviors/PerimeterDocking.cpp | 5 +- .../behaviors/UndockingBehavior.cpp | 5 +- .../src/mower_logic/mower_logic.cpp | 186 +++++++++--------- src/mower_msgs/msg/Status.msg | 3 - src/open_mower/CMakeLists.txt | 15 +- .../launch/include/_localization.launch | 4 +- src/open_mower/launch/include/_teleop.launch | 2 +- src/open_mower/rviz/record_map.rviz | 156 ++++++++++++--- 28 files changed, 454 insertions(+), 262 deletions(-) diff --git a/services/diff_drive_service.json b/services/diff_drive_service.json index aeb5f7b5..1cc05fe9 100644 --- a/services/diff_drive_service.json +++ b/services/diff_drive_service.json @@ -14,26 +14,36 @@ }, { "id": 1, + "name": "Left ESC Status", + "type": "uint8_t" + }, + { + "id": 2, "name": "Left ESC Temperature", "type": "float" }, { - "id": 2, + "id": 3, "name": "Left ESC Current", "type": "float" }, { - "id": 3, + "id": 4, + "name": "Right ESC Status", + "type": "uint8_t" + }, + { + "id": 5, "name": "Right ESC Temperature", "type": "float" }, { - "id": 4, + "id": 6, "name": "Right ESC Current", "type": "float" }, { - "id": 5, + "id": 7, "name": "Wheel Ticks", "type": "uint32_t[2]" } diff --git a/src/lib/xbot_framework b/src/lib/xbot_framework index a2c4330e..c5373ae8 160000 --- a/src/lib/xbot_framework +++ b/src/lib/xbot_framework @@ -1 +1 @@ -Subproject commit a2c4330e4a80dff64cd4d3bf4cb45f1ffa3c74cd +Subproject commit c5373ae81fd6965c3a0764624f2e1bcda8660413 diff --git a/src/mower_comms_v2/src/DiffDriveServiceInterface.cpp b/src/mower_comms_v2/src/DiffDriveServiceInterface.cpp index 0bab6bf8..83c776f5 100644 --- a/src/mower_comms_v2/src/DiffDriveServiceInterface.cpp +++ b/src/mower_comms_v2/src/DiffDriveServiceInterface.cpp @@ -7,7 +7,7 @@ #include #include -bool DiffDriveServiceInterface::OnConfigurationRequested(const std::string& uid) { +bool DiffDriveServiceInterface::OnConfigurationRequested(uint16_t service_id) { StartTransaction(true); SetRegisterWheelDistance(wheel_distance_); SetRegisterWheelTicksPerMeter(ticks_per_meter_); @@ -66,15 +66,26 @@ void DiffDriveServiceInterface::OnLeftESCTemperatureChanged(const float& new_val std::unique_lock lk{state_mutex_}; left_esc_state_.temperature_pcb = new_value; } -void DiffDriveServiceInterface::OnServiceConnected(const std::string& uid) { +void DiffDriveServiceInterface::OnServiceConnected(uint16_t service_id) { std::unique_lock lk{state_mutex_}; - left_esc_state_.status = mower_msgs::ESCStatus::ESC_STATUS_OK; - right_esc_state_.status = mower_msgs::ESCStatus::ESC_STATUS_OK; + 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(const std::string& uid) { +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; diff --git a/src/mower_comms_v2/src/DiffDriveServiceInterface.h b/src/mower_comms_v2/src/DiffDriveServiceInterface.h index d7ae1175..3971608b 100644 --- a/src/mower_comms_v2/src/DiffDriveServiceInterface.h +++ b/src/mower_comms_v2/src/DiffDriveServiceInterface.h @@ -27,7 +27,7 @@ class DiffDriveServiceInterface : public DiffDriveServiceInterfaceBase { ticks_per_meter_(ticks_per_meter) { } - bool OnConfigurationRequested(const std::string& uid) override; + bool OnConfigurationRequested(uint16_t service_id) override; /** * Convenience function to transmit the twist from a ROS message @@ -47,12 +47,14 @@ class DiffDriveServiceInterface : public DiffDriveServiceInterfaceBase { 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(const std::string& uid) override; + void OnServiceConnected(uint16_t service_id) override; void OnTransactionStart(uint64_t timestamp) override; void OnTransactionEnd() override; - void OnServiceDisconnected(const std::string& uid) override; + void OnServiceDisconnected(uint16_t service_id) override; private: // Store the seq number for the actual twist message diff --git a/src/mower_comms_v2/src/DockingSensorServiceInterface.cpp b/src/mower_comms_v2/src/DockingSensorServiceInterface.cpp index f1dbc389..a3675645 100644 --- a/src/mower_comms_v2/src/DockingSensorServiceInterface.cpp +++ b/src/mower_comms_v2/src/DockingSensorServiceInterface.cpp @@ -3,7 +3,7 @@ // #include "DockingSensorServiceInterface.h" -bool DockingSensorServiceInterface::OnConfigurationRequested(const std::string& uid) { +bool DockingSensorServiceInterface::OnConfigurationRequested(uint16_t service_id) { StartTransaction(true); SetRegisterSensorTimeoutMs(1000); CommitTransaction(); @@ -15,7 +15,7 @@ void DockingSensorServiceInterface::OnDetectedSensorsLeftChanged(const uint8_t& void DockingSensorServiceInterface::OnDetectedSensorsRightChanged(const uint8_t& new_value) { msg.detected_right = new_value; } -void DockingSensorServiceInterface::OnServiceConnected(const std::string& uid) { +void DockingSensorServiceInterface::OnServiceConnected(uint16_t service_id) { } void DockingSensorServiceInterface::OnTransactionStart(uint64_t timestamp) { msg = {}; diff --git a/src/mower_comms_v2/src/DockingSensorServiceInterface.h b/src/mower_comms_v2/src/DockingSensorServiceInterface.h index 82482f3e..35640b5f 100644 --- a/src/mower_comms_v2/src/DockingSensorServiceInterface.h +++ b/src/mower_comms_v2/src/DockingSensorServiceInterface.h @@ -16,14 +16,14 @@ class DockingSensorServiceInterface : public DockingSensorServiceInterfaceBase { const ros::Publisher& sensor_publisher) : DockingSensorServiceInterfaceBase(service_id, ctx), sensor_publisher_(sensor_publisher) { } - bool OnConfigurationRequested(const std::string& uid) override; + 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(const std::string& uid) override; + void OnServiceConnected(uint16_t service_id) override; void OnTransactionStart(uint64_t timestamp) override; void OnTransactionEnd() override; const ros::Publisher& sensor_publisher_; diff --git a/src/mower_comms_v2/src/EmergencyServiceInterface.cpp b/src/mower_comms_v2/src/EmergencyServiceInterface.cpp index fe87b7c0..39e6b537 100644 --- a/src/mower_comms_v2/src/EmergencyServiceInterface.cpp +++ b/src/mower_comms_v2/src/EmergencyServiceInterface.cpp @@ -27,7 +27,7 @@ void EmergencyServiceInterface::Heartbeat() { SendSetEmergency(latched_emergency_); } -bool EmergencyServiceInterface::OnConfigurationRequested(const std::string& uid) { +bool EmergencyServiceInterface::OnConfigurationRequested(uint16_t service_id) { // No config needed return true; } @@ -56,13 +56,13 @@ void EmergencyServiceInterface::OnTransactionEnd() { PublishEmergencyState(); } -void EmergencyServiceInterface::OnServiceConnected(const std::string& uid) { +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(const std::string& uid) { +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"; diff --git a/src/mower_comms_v2/src/EmergencyServiceInterface.h b/src/mower_comms_v2/src/EmergencyServiceInterface.h index 678326e5..557aaa68 100644 --- a/src/mower_comms_v2/src/EmergencyServiceInterface.h +++ b/src/mower_comms_v2/src/EmergencyServiceInterface.h @@ -21,16 +21,16 @@ class EmergencyServiceInterface : public EmergencyServiceInterfaceBase { void Heartbeat(); protected: - bool OnConfigurationRequested(const std::string& uid) override; + 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(const std::string& uid) override; + void OnServiceConnected(uint16_t service_id) override; void OnTransactionStart(uint64_t timestamp) override; void OnTransactionEnd() override; - void OnServiceDisconnected(const std::string& uid) override; + void OnServiceDisconnected(uint16_t service_id) override; void PublishEmergencyState(); diff --git a/src/mower_comms_v2/src/ImuServiceInterface.cpp b/src/mower_comms_v2/src/ImuServiceInterface.cpp index a6d11fb4..78b71509 100644 --- a/src/mower_comms_v2/src/ImuServiceInterface.cpp +++ b/src/mower_comms_v2/src/ImuServiceInterface.cpp @@ -4,7 +4,7 @@ #include "ImuServiceInterface.h" -bool ImuServiceInterface::OnConfigurationRequested(const std::string& uid) { +bool ImuServiceInterface::OnConfigurationRequested(uint16_t service_id) { return true; } void ImuServiceInterface::OnAxesChanged(const double* new_value, uint32_t length) { @@ -22,11 +22,11 @@ void ImuServiceInterface::OnAxesChanged(const double* new_value, uint32_t length imu_msg.angular_velocity.z = new_value[5]; imu_publisher_.publish(imu_msg); } -void ImuServiceInterface::OnServiceConnected(const std::string& uid) { +void ImuServiceInterface::OnServiceConnected(uint16_t service_id) { } void ImuServiceInterface::OnTransactionStart(uint64_t timestamp) { } void ImuServiceInterface::OnTransactionEnd() { } -void ImuServiceInterface::OnServiceDisconnected(const std::string& uid) { +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 index 022afdf2..2b84f6f3 100644 --- a/src/mower_comms_v2/src/ImuServiceInterface.h +++ b/src/mower_comms_v2/src/ImuServiceInterface.h @@ -15,16 +15,16 @@ class ImuServiceInterface : public ImuServiceInterfaceBase { 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(const std::string& uid) override; + bool OnConfigurationRequested(uint16_t service_id) override; protected: void OnAxesChanged(const double* new_value, uint32_t length) override; private: - void OnServiceConnected(const std::string& uid) override; + void OnServiceConnected(uint16_t service_id) override; void OnTransactionStart(uint64_t timestamp) override; void OnTransactionEnd() override; - void OnServiceDisconnected(const std::string& uid) override; + void OnServiceDisconnected(uint16_t service_id) override; const ros::Publisher& imu_publisher_; sensor_msgs::Imu imu_msg{}; diff --git a/src/mower_comms_v2/src/LidarServiceInterface.cpp b/src/mower_comms_v2/src/LidarServiceInterface.cpp index 7db19bb2..33ee464d 100644 --- a/src/mower_comms_v2/src/LidarServiceInterface.cpp +++ b/src/mower_comms_v2/src/LidarServiceInterface.cpp @@ -6,7 +6,7 @@ #include "spdlog/spdlog.h" -bool LidarServiceInterface::OnConfigurationRequested(const std::string &uid) { +bool LidarServiceInterface::OnConfigurationRequested(uint16_t service_id) { return true; } @@ -85,9 +85,6 @@ void LidarServiceInterface::OnTransactionEnd() { 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(); - 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(); // our laser scanner does sometimes report weird start/end angles, so we set min-max angles manually // TODO: either fix, or make configurable @@ -95,6 +92,10 @@ void LidarServiceInterface::OnTransactionEnd() { 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? diff --git a/src/mower_comms_v2/src/LidarServiceInterface.h b/src/mower_comms_v2/src/LidarServiceInterface.h index ed4a74a7..85988f68 100644 --- a/src/mower_comms_v2/src/LidarServiceInterface.h +++ b/src/mower_comms_v2/src/LidarServiceInterface.h @@ -11,49 +11,50 @@ #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) { - } +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(const std::string &uid) override; + bool OnConfigurationRequested(uint16_t service_id) override; - protected: - void OnAngleMinRadChanged(const double &new_value) override; +protected: + void OnAngleMinRadChanged(const double &new_value) override; - void OnAngleMaxRadChanged(const double &new_value) override; + void OnAngleMaxRadChanged(const double &new_value) override; - void OnTimeIncrementSecondsChanged(const float &new_value) override; + void OnTimeIncrementSecondsChanged(const float &new_value) override; - void OnScanTimeSecondsChanged(const float &new_value) override; + void OnScanTimeSecondsChanged(const float &new_value) override; - void OnRangeMinMChanged(const float &new_value) override; + void OnRangeMinMChanged(const float &new_value) override; - void OnRangeMaxMChanged(const float &new_value) override; + void OnRangeMaxMChanged(const float &new_value) override; - void OnRangesChanged(const float *new_value, uint32_t length) 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; + void OnIntensitiesChanged(const float *new_value, uint32_t length) override; - private: - void OnTransactionEnd() override; + void OnNewScanChanged(const uint8_t &new_value) override; - void OnTransactionStart(uint64_t timestamp) override; +private: + void OnTransactionEnd() 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}; + void OnTransactionStart(uint64_t timestamp) override; - sensor_msgs::LaserScan laser_scan_msg_{}; + 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 index a524df56..62c04e68 100644 --- a/src/mower_comms_v2/src/MowerServiceInterface.cpp +++ b/src/mower_comms_v2/src/MowerServiceInterface.cpp @@ -4,7 +4,7 @@ #include "MowerServiceInterface.h" -bool MowerServiceInterface::OnConfigurationRequested(const std::string& uid) { +bool MowerServiceInterface::OnConfigurationRequested(uint16_t service_id) { // No configuration return true; } @@ -32,7 +32,7 @@ void MowerServiceInterface::OnMowerMotorCurrentChanged(const float& new_value) { void MowerServiceInterface::OnMowerMotorRPMChanged(const float& new_value) { status_msg_.mower_motor_rpm = new_value; } -void MowerServiceInterface::OnServiceConnected(const std::string& uid) { +void MowerServiceInterface::OnServiceConnected(uint16_t service_id) { status_msg_ = {}; } void MowerServiceInterface::OnTransactionStart(uint64_t timestamp) { @@ -41,5 +41,5 @@ void MowerServiceInterface::OnTransactionStart(uint64_t timestamp) { void MowerServiceInterface::OnTransactionEnd() { status_publisher_.publish(status_msg_); } -void MowerServiceInterface::OnServiceDisconnected(const std::string& uid) { +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 index 040d0f28..d96d15d1 100644 --- a/src/mower_comms_v2/src/MowerServiceInterface.h +++ b/src/mower_comms_v2/src/MowerServiceInterface.h @@ -17,7 +17,7 @@ class MowerServiceInterface : public MowerServiceInterfaceBase { : MowerServiceInterfaceBase(service_id, ctx), status_publisher_(status_publisher) { } - bool OnConfigurationRequested(const std::string& uid) override; + bool OnConfigurationRequested(uint16_t service_id) override; void SetMowerEnabled(bool enabled); protected: @@ -30,10 +30,10 @@ class MowerServiceInterface : public MowerServiceInterfaceBase { void OnMowerMotorRPMChanged(const float& new_value) override; private: - void OnServiceConnected(const std::string& uid) override; + void OnServiceConnected(uint16_t service_id) override; void OnTransactionStart(uint64_t timestamp) override; void OnTransactionEnd() override; - void OnServiceDisconnected(const std::string& uid) override; + void OnServiceDisconnected(uint16_t service_id) override; private: mower_msgs::Status status_msg_{}; diff --git a/src/mower_comms_v2/src/PowerServiceInterface.cpp b/src/mower_comms_v2/src/PowerServiceInterface.cpp index d009cb5f..ea8b9ec8 100644 --- a/src/mower_comms_v2/src/PowerServiceInterface.cpp +++ b/src/mower_comms_v2/src/PowerServiceInterface.cpp @@ -3,7 +3,7 @@ // #include "PowerServiceInterface.h" -bool PowerServiceInterface::OnConfigurationRequested(const std::string& uid) { +bool PowerServiceInterface::OnConfigurationRequested(uint16_t service_id) { return true; } void PowerServiceInterface::OnChargeVoltageChanged(const float& new_value) { diff --git a/src/mower_comms_v2/src/PowerServiceInterface.h b/src/mower_comms_v2/src/PowerServiceInterface.h index 6c4b147a..1b36e69d 100644 --- a/src/mower_comms_v2/src/PowerServiceInterface.h +++ b/src/mower_comms_v2/src/PowerServiceInterface.h @@ -16,7 +16,7 @@ class PowerServiceInterface : public PowerServiceInterfaceBase { const ros::Publisher& status_publisher) : PowerServiceInterfaceBase(service_id, ctx), status_publisher_(status_publisher) { } - bool OnConfigurationRequested(const std::string& uid) override; + bool OnConfigurationRequested(uint16_t service_id) override; protected: void OnChargeVoltageChanged(const float& new_value) override; diff --git a/src/mower_logic/CMakeLists.txt b/src/mower_logic/CMakeLists.txt index 520f37ad..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,7 +181,7 @@ 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}) diff --git a/src/mower_logic/src/monitoring/monitoring.cpp b/src/mower_logic/src/monitoring/monitoring.cpp index ee629f65..1b412e67 100644 --- a/src/mower_logic/src/monitoring/monitoring.cpp +++ b/src/mower_logic/src/monitoring/monitoring.cpp @@ -16,6 +16,10 @@ // // +#include +#include +#include + #include "mower_msgs/HighLevelStatus.h" #include "mower_msgs/Status.h" #include "ros/ros.h" @@ -39,6 +43,10 @@ xbot_msgs::SensorInfo si_charge_current; ros::Publisher si_charge_current_pub; ros::Publisher charge_current_data_pub; +xbot_msgs::SensorInfo si_charge_mode; +ros::Publisher si_charge_mode_pub; +ros::Publisher charge_mode_data_pub; + xbot_msgs::SensorInfo si_left_esc_temp; ros::Publisher si_left_esc_temp_pub; ros::Publisher left_esc_temp_data_pub; @@ -65,14 +73,26 @@ ros::Publisher gps_accuracy_data_pub; ros::NodeHandle *n; -ros::Time last_status_update(0); -ros::Time last_pose_update(0); - void status(const mower_msgs::Status::ConstPtr &msg) { + static ros::Time last_update(0); // Rate limit to 2Hz - if ((msg->stamp - last_status_update).toSec() < 0.5) return; - last_status_update = msg->stamp; + 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->mower_esc_temperature; + mow_esc_temp_data_pub.publish(sensor_data); + + sensor_data.data = msg->mower_motor_temperature; + mow_motor_temp_data_pub.publish(sensor_data); + sensor_data.data = msg->mower_esc_current; + mow_motor_current_data_pub.publish(sensor_data); +} + +void power_received(const mower_msgs::Power::ConstPtr &msg) { xbot_msgs::SensorDataDouble sensor_data; sensor_data.stamp = msg->stamp; @@ -85,20 +105,34 @@ void status(const mower_msgs::Status::ConstPtr &msg) { sensor_data.data = msg->charge_current; charge_current_data_pub.publish(sensor_data); - sensor_data.data = msg->left_esc_status.temperature_pcb; - left_esc_temp_data_pub.publish(sensor_data); + xbot_msgs::SensorDataString status_sensor_data; + status_sensor_data.stamp = msg->stamp; + status_sensor_data.data = msg->charger_status; + charge_mode_data_pub.publish(status_sensor_data); +} - sensor_data.data = msg->right_esc_status.temperature_pcb; - right_esc_temp_data_pub.publish(sensor_data); +void left_esc_status(const mower_msgs::ESCStatus::ConstPtr &msg) { + static ros::Time last_update(0); + // Rate limit to 2Hz + if ((ros::Time::now() - last_update).toSec() < 0.5) return; + last_update = ros::Time::now(); - sensor_data.data = msg->mow_esc_status.temperature_pcb; - mow_esc_temp_data_pub.publish(sensor_data); + xbot_msgs::SensorDataDouble sensor_data; + sensor_data.stamp = ros::Time::now(); + sensor_data.data = msg->temperature_pcb; + left_esc_temp_data_pub.publish(sensor_data); +} - sensor_data.data = msg->mow_esc_status.temperature_motor; - mow_motor_temp_data_pub.publish(sensor_data); +void right_esc_status(const mower_msgs::ESCStatus::ConstPtr &msg) { + static ros::Time last_update(0); + // Rate limit to 2Hz + if ((ros::Time::now() - last_update).toSec() < 0.5) return; + last_update = ros::Time::now(); - sensor_data.data = msg->mow_esc_status.current; - mow_motor_current_data_pub.publish(sensor_data); + xbot_msgs::SensorDataDouble sensor_data; + sensor_data.stamp = ros::Time::now(); + sensor_data.data = msg->temperature_pcb; + right_esc_temp_data_pub.publish(sensor_data); } void high_level_status(const mower_msgs::HighLevelStatus::ConstPtr &msg) { @@ -118,9 +152,10 @@ void high_level_status(const mower_msgs::HighLevelStatus::ConstPtr &msg) { void pose_received(const xbot_msgs::AbsolutePose::ConstPtr &msg) { state.robot_pose = *msg; + static ros::Time last_update(0); // Rate limit to 2Hz - if ((msg->header.stamp - last_pose_update).toSec() < 0.5) return; - last_pose_update = msg->header.stamp; + if ((ros::Time::now() - last_update).toSec() < 0.5) return; + last_update = ros::Time::now(); xbot_msgs::SensorDataDouble sensor_data; sensor_data.stamp = msg->header.stamp; @@ -162,6 +197,17 @@ void registerSensors() { n->advertise("xbot_monitoring/sensors/" + si_charge_current.sensor_id + "/data", 10); si_charge_current_pub.publish(si_charge_current); + si_charge_mode.sensor_id = "om_charge_mode"; + si_charge_mode.sensor_name = "Charge Mode"; + si_charge_mode.value_type = xbot_msgs::SensorInfo::TYPE_STRING; + si_charge_mode.value_description = xbot_msgs::SensorInfo::VALUE_DESCRIPTION_UNKNOWN; + si_charge_mode.unit = ""; + si_charge_mode_pub = + n->advertise("xbot_monitoring/sensors/" + si_charge_mode.sensor_id + "/info", 1, true); + charge_mode_data_pub = + n->advertise("xbot_monitoring/sensors/" + si_charge_mode.sensor_id + "/data", 10); + si_charge_mode_pub.publish(si_charge_mode); + si_left_esc_temp.sensor_id = "om_left_esc_temp"; si_left_esc_temp.sensor_name = "Left ESC Temp"; si_left_esc_temp.value_type = xbot_msgs::SensorInfo::TYPE_DOUBLE; @@ -239,6 +285,9 @@ int main(int argc, char **argv) { ros::Subscriber pose_sub = n->subscribe("xbot_positioning/xb_pose", 10, pose_received); ros::Subscriber state_sub = n->subscribe("mower_logic/current_state", 10, high_level_status); ros::Subscriber status_sub = n->subscribe("mower/status", 10, status); + ros::Subscriber left_esc_status_sub = n->subscribe("ll/diff_drive/left_esc_status", 10, left_esc_status); + ros::Subscriber right_esc_status_sub = n->subscribe("ll/diff_drive/right_esc_status", 10, right_esc_status); + ros::Subscriber power_status_sub = n->subscribe("ll/power", 10, power_received); state_pub = n->advertise("xbot_monitoring/robot_state", 10); 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 49ee6b16..c3f5cf63 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(); @@ -28,6 +30,7 @@ extern void registerActions(std::string prefix, const std::vector *reconfigServer; @@ -69,18 +72,19 @@ 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 && shared_state->active_semiautomatic_task && !shared_state->semiautomatic_task_paused; - 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; 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); @@ -100,7 +104,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 a061d94e..91787029 100644 --- a/src/mower_logic/src/mower_logic/behaviors/UndockingBehavior.cpp +++ b/src/mower_logic/src/mower_logic/behaviors/UndockingBehavior.cpp @@ -17,10 +17,13 @@ // #include "UndockingBehavior.h" +#include + 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(); @@ -99,7 +102,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 91ec0ea3..febd9c8a 100644 --- a/src/mower_logic/src/mower_logic/mower_logic.cpp +++ b/src/mower_logic/src/mower_logic/mower_logic.cpp @@ -20,7 +20,9 @@ #include #include +#include #include +#include #include #include @@ -28,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" @@ -40,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" @@ -71,12 +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; -mower_msgs::Emergency last_emergency; +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); @@ -95,16 +93,6 @@ double max_v_battery_seen = 0.0; /** * 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; @@ -115,16 +103,6 @@ void setLastGoodGPS(ros::Time time) { last_good_gps = time; } -mower_msgs::Emergency getEmergency() { - std::lock_guard lk{mower_logic_mutex}; - return last_emergency; -} - -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; @@ -136,9 +114,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); @@ -162,10 +145,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; @@ -188,32 +170,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 emergencyReceived(const mower_msgs::Emergency &msg) { - std::lock_guard lk{mower_logic_mutex}; - last_emergency = msg; -} - -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) { @@ -259,6 +215,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; @@ -400,6 +357,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); } @@ -408,16 +366,22 @@ 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_emergency = getEmergency(); + 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_emergency.latched_emergency; - high_level_status.is_charging = last_status.v_charge > 10.0; + 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; @@ -428,7 +392,7 @@ void checkSafety(const ros::TimerEvent &timer_event) { 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); @@ -452,7 +416,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)); @@ -460,11 +424,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; } @@ -506,7 +470,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; @@ -526,13 +490,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; @@ -542,8 +506,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; } @@ -651,9 +615,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"); @@ -678,11 +639,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 emergency_sub = - n->subscribe("/mower/emergency", 0, emergencyReceived, 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)); @@ -693,8 +656,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); @@ -705,7 +689,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); @@ -818,7 +822,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_emergency.latched_emergency) { + if (emergency_state_subscriber.getMessage().latched_emergency) { ROS_INFO_STREAM("Got emergency, resetting it"); setEmergencyMode(false); break; diff --git a/src/mower_msgs/msg/Status.msg b/src/mower_msgs/msg/Status.msg index 1f7bc1bd..68de0a7a 100644 --- a/src/mower_msgs/msg/Status.msg +++ b/src/mower_msgs/msg/Status.msg @@ -14,9 +14,6 @@ bool rain_detected bool sound_module_available bool sound_module_busy bool ui_board_available -float32 v_charge -float32 v_battery -float32 charge_current bool mow_enabled # ESC stuff 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/_localization.launch b/src/open_mower/launch/include/_localization.launch index 93146fd0..d64bc8ff 100644 --- a/src/open_mower/launch/include/_localization.launch +++ b/src/open_mower/launch/include/_localization.launch @@ -1,6 +1,8 @@ - + + + diff --git a/src/open_mower/launch/include/_teleop.launch b/src/open_mower/launch/include/_teleop.launch index c0ee22a3..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/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 From f8b91f9b463ea0cceaf73291ae5c1b7a7f571d2f Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Fri, 29 Nov 2024 13:16:56 +0100 Subject: [PATCH 21/29] update xbot_positioning --- src/lib/xbot_positioning | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/xbot_positioning b/src/lib/xbot_positioning index a034f54e..3d667f24 160000 --- a/src/lib/xbot_positioning +++ b/src/lib/xbot_positioning @@ -1 +1 @@ -Subproject commit a034f54e1c33b3eec924d6de9b450b943ecb14f9 +Subproject commit 3d667f24f6d89da028b2f9c55407134fcaefd6f9 From 01a240daab15918bc5636c33918960ada8eb61c5 Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Fri, 29 Nov 2024 15:06:35 +0100 Subject: [PATCH 22/29] fixed build --- src/lib/ntrip_client | 2 +- src/lib/slic3r_coverage_planner | 2 +- src/lib/xbot_driver_gps | 2 +- src/lib/xbot_framework | 2 +- src/lib/xbot_monitoring | 2 +- src/lib/xbot_msgs | 2 +- src/lib/xesc_ros | 2 +- .../src/mower_logic/StateSubscriber.h | 70 +++++++++++++++++++ 8 files changed, 77 insertions(+), 7 deletions(-) create mode 100644 src/mower_logic/src/mower_logic/StateSubscriber.h diff --git a/src/lib/ntrip_client b/src/lib/ntrip_client index 8a8b61ec..b18cafee 160000 --- a/src/lib/ntrip_client +++ b/src/lib/ntrip_client @@ -1 +1 @@ -Subproject commit 8a8b61eca3db76b961a2bd9f581ccb7576033f25 +Subproject commit b18cafee5cd0b996447be14ec70b3d3e8c288530 diff --git a/src/lib/slic3r_coverage_planner b/src/lib/slic3r_coverage_planner index 195b8f72..ba7bb2a3 160000 --- a/src/lib/slic3r_coverage_planner +++ b/src/lib/slic3r_coverage_planner @@ -1 +1 @@ -Subproject commit 195b8f72cbc163a1e50c04f21a09fd2871ec6e91 +Subproject commit ba7bb2a301af5eff9d797bc23966e77f532b70bb diff --git a/src/lib/xbot_driver_gps b/src/lib/xbot_driver_gps index 76fe27d6..9d7b18cf 160000 --- a/src/lib/xbot_driver_gps +++ b/src/lib/xbot_driver_gps @@ -1 +1 @@ -Subproject commit 76fe27d6474789693525ac53027f4ee32be1eaed +Subproject commit 9d7b18cf018435ee4669886e917a85b053abb743 diff --git a/src/lib/xbot_framework b/src/lib/xbot_framework index c5373ae8..62db7e73 160000 --- a/src/lib/xbot_framework +++ b/src/lib/xbot_framework @@ -1 +1 @@ -Subproject commit c5373ae81fd6965c3a0764624f2e1bcda8660413 +Subproject commit 62db7e73f62172f76af19cb13f645ccf2109c038 diff --git a/src/lib/xbot_monitoring b/src/lib/xbot_monitoring index 3912d8be..e51841df 160000 --- a/src/lib/xbot_monitoring +++ b/src/lib/xbot_monitoring @@ -1 +1 @@ -Subproject commit 3912d8be1109f660cecf18e7d31797432b1d2979 +Subproject commit e51841df355c4819ed6bd3443f7b4e3381f2e1ad diff --git a/src/lib/xbot_msgs b/src/lib/xbot_msgs index 1c8ffb7a..17b415e6 160000 --- a/src/lib/xbot_msgs +++ b/src/lib/xbot_msgs @@ -1 +1 @@ -Subproject commit 1c8ffb7a3e5288b9c1a1653080378bf2fedef668 +Subproject commit 17b415e6c6244f0d07b0bdac7bd5c486fd252e50 diff --git a/src/lib/xesc_ros b/src/lib/xesc_ros index 28aeda55..a064beff 160000 --- a/src/lib/xesc_ros +++ b/src/lib/xesc_ros @@ -1 +1 @@ -Subproject commit 28aeda55f02f15ae3f816d9462ac2cddf42a58aa +Subproject commit a064beff7a46846431e602c4efe2bc286bcfd5c5 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 From 51c4a6715c4432d5cb87db70c66ed5843dae60c6 Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Fri, 29 Nov 2024 15:45:28 +0100 Subject: [PATCH 23/29] fixed build --- src/mower_comms_v1/CMakeLists.txt | 5 - src/mower_comms_v1/src/mower_comms.cpp | 40 ++++++-- src/mower_logic/src/monitoring/monitoring.cpp | 99 ++++++++++++++----- src/mower_msgs/msg/Status.msg | 1 + 4 files changed, 112 insertions(+), 33 deletions(-) diff --git a/src/mower_comms_v1/CMakeLists.txt b/src/mower_comms_v1/CMakeLists.txt index 1b350dad..6b396753 100644 --- a/src/mower_comms_v1/CMakeLists.txt +++ b/src/mower_comms_v1/CMakeLists.txt @@ -23,9 +23,6 @@ find_package( Boost ) -add_service(EmergencyService ${CMAKE_CURRENT_SOURCE_DIR}/../../services/emergency_service.json) -add_service(DiffDriveService ${CMAKE_CURRENT_SOURCE_DIR}/../../services/diff_drive_service.json) - ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -171,8 +168,6 @@ add_executable(mower_comms_v1 add_dependencies(mower_comms_v1 ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(mower_comms_v1 ${catkin_LIBRARIES} - EmergencyService - DiffDriveService ) ############# diff --git a/src/mower_comms_v1/src/mower_comms.cpp b/src/mower_comms_v1/src/mower_comms.cpp index 3da16f8c..c6e94a7e 100644 --- a/src/mower_comms_v1/src/mower_comms.cpp +++ b/src/mower_comms_v1/src/mower_comms.cpp @@ -28,6 +28,8 @@ #include #include +#include +#include #include "COBS.h" #include "boost/crc.hpp" @@ -173,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(); @@ -212,9 +236,10 @@ void publishStatus() { emergency_msg.reason = ""; emergency_pub.publish(emergency_msg); - 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::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) { @@ -225,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); 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_msgs/msg/Status.msg b/src/mower_msgs/msg/Status.msg index 68de0a7a..954f20c0 100644 --- a/src/mower_msgs/msg/Status.msg +++ b/src/mower_msgs/msg/Status.msg @@ -17,6 +17,7 @@ bool ui_board_available bool mow_enabled # ESC stuff +uint8 mower_esc_status float32 mower_esc_temperature float32 mower_esc_current float32 mower_motor_temperature From 06f4033317055925f0ce5fa6195dc075da2c9d5d Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Sat, 30 Nov 2024 22:15:26 +0100 Subject: [PATCH 24/29] Implemented robot simulation using v2 protocol --- ...gps_service.json => position_service.json} | 4 +- src/lib/xbot_framework | 2 +- src/lib/xbot_positioning | 2 +- src/mower_comms_v2/CMakeLists.txt | 3 + .../src/GpsPositionServiceInterface.cpp | 86 +++++++ .../src/GpsPositionServiceInterface.h | 40 +++ src/mower_comms_v2/src/mower_comms.cpp | 6 + src/mower_simulation/CMakeLists.txt | 22 +- src/mower_simulation/src/SimRobot.cpp | 153 ++++++++++++ src/mower_simulation/src/SimRobot.h | 96 ++++++++ src/mower_simulation/src/mower_simulation.cpp | 229 +++--------------- .../diff_drive_service/diff_drive_service.cpp | 42 ++++ .../diff_drive_service/diff_drive_service.hpp | 37 +++ .../emergency_service/emergency_service.cpp | 61 +++++ .../emergency_service/emergency_service.hpp | 38 +++ .../src/services/imu_service/imu_service.cpp | 20 ++ .../src/services/imu_service/imu_service.hpp | 27 +++ .../services/mower_service/mower_service.cpp | 28 +++ .../services/mower_service/mower_service.hpp | 30 +++ .../position_service/position_service.cpp | 37 +++ .../position_service/position_service.hpp | 33 +++ .../services/power_service/power_service.cpp | 35 +++ .../services/power_service/power_service.hpp | 42 ++++ .../launch/include/_localization.launch | 5 +- 24 files changed, 870 insertions(+), 208 deletions(-) rename services/{gps_service.json => position_service.json} (94%) create mode 100644 src/mower_comms_v2/src/GpsPositionServiceInterface.cpp create mode 100644 src/mower_comms_v2/src/GpsPositionServiceInterface.h create mode 100644 src/mower_simulation/src/SimRobot.cpp create mode 100644 src/mower_simulation/src/SimRobot.h create mode 100644 src/mower_simulation/src/services/diff_drive_service/diff_drive_service.cpp create mode 100644 src/mower_simulation/src/services/diff_drive_service/diff_drive_service.hpp create mode 100644 src/mower_simulation/src/services/emergency_service/emergency_service.cpp create mode 100644 src/mower_simulation/src/services/emergency_service/emergency_service.hpp create mode 100644 src/mower_simulation/src/services/imu_service/imu_service.cpp create mode 100644 src/mower_simulation/src/services/imu_service/imu_service.hpp create mode 100644 src/mower_simulation/src/services/mower_service/mower_service.cpp create mode 100644 src/mower_simulation/src/services/mower_service/mower_service.hpp create mode 100644 src/mower_simulation/src/services/position_service/position_service.cpp create mode 100644 src/mower_simulation/src/services/position_service/position_service.hpp create mode 100644 src/mower_simulation/src/services/power_service/power_service.cpp create mode 100644 src/mower_simulation/src/services/power_service/power_service.hpp diff --git a/services/gps_service.json b/services/position_service.json similarity index 94% rename from services/gps_service.json rename to services/position_service.json index 1ce84590..abbbfca5 100644 --- a/services/gps_service.json +++ b/services/position_service.json @@ -24,7 +24,7 @@ }, { "id": 3, - "name": "MotionVectorENU", + "name": "MotionVectorXYZ", "type": "double[3]" }, { @@ -60,6 +60,6 @@ "type": "double[3]" } ], - "type": "GpsService", + "type": "PositionService", "version": 1 } diff --git a/src/lib/xbot_framework b/src/lib/xbot_framework index 62db7e73..88ca8b4e 160000 --- a/src/lib/xbot_framework +++ b/src/lib/xbot_framework @@ -1 +1 @@ -Subproject commit 62db7e73f62172f76af19cb13f645ccf2109c038 +Subproject commit 88ca8b4edcb49da7b2a1cc1ca67466d0d7d1b02a diff --git a/src/lib/xbot_positioning b/src/lib/xbot_positioning index 3d667f24..7e80cd3a 160000 --- a/src/lib/xbot_positioning +++ b/src/lib/xbot_positioning @@ -1 +1 @@ -Subproject commit 3d667f24f6d89da028b2f9c55407134fcaefd6f9 +Subproject commit 7e80cd3af894c64128fcd62d12d2de050f37301a diff --git a/src/mower_comms_v2/CMakeLists.txt b/src/mower_comms_v2/CMakeLists.txt index 53b1575c..d82cdb57 100644 --- a/src/mower_comms_v2/CMakeLists.txt +++ b/src/mower_comms_v2/CMakeLists.txt @@ -173,6 +173,8 @@ add_executable(mower_comms_v2 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) @@ -182,6 +184,7 @@ target_add_service_interface(mower_comms_v2 MowerServiceInterface ${CMAKE_CURREN 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}) 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/mower_comms.cpp b/src/mower_comms_v2/src/mower_comms.cpp index dbf96864..0cc23e40 100644 --- a/src/mower_comms_v2/src/mower_comms.cpp +++ b/src/mower_comms_v2/src/mower_comms.cpp @@ -28,6 +28,7 @@ #include "DiffDriveServiceInterface.h" #include "DockingSensorServiceInterface.h" #include "EmergencyServiceInterface.h" +#include "GpsPositionServiceInterface.h" #include "ImuServiceInterface.h" #include "LidarServiceInterface.h" #include "MowerServiceInterface.h" @@ -35,6 +36,7 @@ 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; @@ -53,6 +55,7 @@ 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{}; @@ -109,6 +112,7 @@ int main(int argc, char **argv) { 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)); @@ -123,6 +127,7 @@ int main(int argc, char **argv) { 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); @@ -133,6 +138,7 @@ int main(int argc, char **argv) { lidar_service->Start(); power_service->Start(); docking_sensor_service->Start(); + gps_position_service->Start(); ros::spin(); 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/src/SimRobot.cpp b/src/mower_simulation/src/SimRobot.cpp new file mode 100644 index 00000000..52456ede --- /dev/null +++ b/src/mower_simulation/src/SimRobot.cpp @@ -0,0 +1,153 @@ +// +// 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) { + const auto now = ros::Time::now(); + // Update Position + 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/launch/include/_localization.launch b/src/open_mower/launch/include/_localization.launch index d64bc8ff..44fa9d3e 100644 --- a/src/open_mower/launch/include/_localization.launch +++ b/src/open_mower/launch/include/_localization.launch @@ -2,12 +2,11 @@ - - - + + From 046ba75e1e7d2abeea1f18efb309575e6c7e33af Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Sat, 30 Nov 2024 22:29:02 +0100 Subject: [PATCH 25/29] Removed old robot simulator --- src/mower_simulation/launch/_mower_simulation.launch | 7 ------- src/mower_simulation/package.xml | 1 - 2 files changed, 8 deletions(-) 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 From dc6b1be40a06220290499c8d7fc5206fab863a50 Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Sat, 30 Nov 2024 22:29:19 +0100 Subject: [PATCH 26/29] SimRobot now not moving when emergency is active --- src/mower_simulation/src/SimRobot.cpp | 47 ++++++++++++++------------- 1 file changed, 25 insertions(+), 22 deletions(-) diff --git a/src/mower_simulation/src/SimRobot.cpp b/src/mower_simulation/src/SimRobot.cpp index 52456ede..d49afcf9 100644 --- a/src/mower_simulation/src/SimRobot.cpp +++ b/src/mower_simulation/src/SimRobot.cpp @@ -66,9 +66,9 @@ void SimRobot::GetPosition(double& x, double& y, double& 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; + 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, @@ -82,45 +82,48 @@ void SimRobot::GetIsCharging(bool& charging, double& seconds_since_start, std::s 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 - 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 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_) { + 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_) { + if (is_charging_) { spdlog::info("Stopped Charging"); is_charging_ = false; } } - if(is_charging_) { - if(battery_volts_ < BATTERY_VOLTS_MAX) { + if (is_charging_) { + if (battery_volts_ < BATTERY_VOLTS_MAX) { charger_state_ = "CC"; battery_volts_ += 0.05; - if(battery_volts_ > BATTERY_VOLTS_MAX) { + 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) { + } else if (charge_current_ > 0.2) { charger_state_ = "CV"; battery_volts_ = BATTERY_VOLTS_MAX; charger_volts_ = CHARGE_VOLTS; From 1c4a95deb9912c89d5f6b28b83dbd8f6e405521a Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Sat, 30 Nov 2024 22:29:24 +0100 Subject: [PATCH 27/29] fixed Dockerfile --- docker/Dockerfile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index de55cda4..77f5c036 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -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 From 8a42fe27212b6f3cbeb03ce2f9600b2bc1040b7b Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Sat, 30 Nov 2024 22:49:06 +0100 Subject: [PATCH 28/29] wip --- docker/Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index 77f5c036..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/* From 18a11848b8803d94bc3518cf798b56ac47d7fd78 Mon Sep 17 00:00:00 2001 From: Clemens Elflein Date: Thu, 12 Dec 2024 15:07:34 +0100 Subject: [PATCH 29/29] Updated xbot_framework --- src/lib/xbot_framework | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/xbot_framework b/src/lib/xbot_framework index 88ca8b4e..b56fd70e 160000 --- a/src/lib/xbot_framework +++ b/src/lib/xbot_framework @@ -1 +1 @@ -Subproject commit 88ca8b4edcb49da7b2a1cc1ca67466d0d7d1b02a +Subproject commit b56fd70ef32b3baf72c12a87cf35eb5b0f053eec