diff --git a/.github/workflows/slate-humble.yaml b/.github/workflows/slate-humble.yaml index 914eb32..7e9679d 100644 --- a/.github/workflows/slate-humble.yaml +++ b/.github/workflows/slate-humble.yaml @@ -24,6 +24,7 @@ jobs: - uses: actions/checkout@v4 with: path: src/interbotix_ros_core + submodules: recursive - name: Prepare Workspace run: | rm src/interbotix_ros_core/interbotix_ros_slate/COLCON_IGNORE diff --git a/.gitmodules b/.gitmodules index e49f002..ad92657 100644 --- a/.gitmodules +++ b/.gitmodules @@ -6,3 +6,6 @@ path = interbotix_ros_xseries/interbotix_xs_driver url = https://github.com/Interbotix/interbotix_xs_driver.git branch = v0.3.3 +[submodule "interbotix_ros_slate/trossen_slate"] + path = interbotix_ros_slate/trossen_slate + url = https://github.com/Interbotix/trossen_slate.git diff --git a/interbotix_ros_slate/interbotix_slate_driver/CMakeLists.txt b/interbotix_ros_slate/interbotix_slate_driver/CMakeLists.txt index 691bf11..1f2b64f 100644 --- a/interbotix_ros_slate/interbotix_slate_driver/CMakeLists.txt +++ b/interbotix_ros_slate/interbotix_slate_driver/CMakeLists.txt @@ -11,8 +11,6 @@ endif() set(CMAKE_BUILD_TYPE "Release") -set(serial_driver "chassis_driver") - find_package(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) find_package(interbotix_slate_msgs REQUIRED) @@ -23,6 +21,7 @@ find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) +find_package(trossen_slate REQUIRED) include_directories(include) @@ -36,56 +35,19 @@ set(ROS_DEPENDENCIES std_srvs tf2_geometry_msgs tf2_ros -) - -if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64") - set(ARCH "x86_64") -elseif(${CMAKE_SYSTEM_PROCESSOR} MATCHES "aarch64") - set(ARCH "aarch64") -else() - message(FATAL_ERROR "Unknown System Architecture: ${CMAKE_SYSTEM_PROCESSOR}") -endif() - -link_directories(${CMAKE_CURRENT_SOURCE_DIR}/lib/${ARCH}) - -add_library(slate_base - src/slate_base.cpp - src/base_driver.cpp -) - -ament_target_dependencies(slate_base ${ROS_DEPENDENCIES}) - -target_link_libraries(slate_base - ${serial_driver} + trossen_slate ) add_executable(slate_base_node src/slate_base_node.cpp + src/slate_base.cpp ) -target_link_libraries(slate_base_node - slate_base -) - -install( - TARGETS slate_base - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -install( - TARGETS - slate_base_node - RUNTIME DESTINATION - lib/${PROJECT_NAME} -) +ament_target_dependencies(slate_base_node ${ROS_DEPENDENCIES}) install( - FILES - ${CMAKE_CURRENT_SOURCE_DIR}/lib/${ARCH}/lib${serial_driver}.so - DESTINATION - lib + TARGETS slate_base_node + DESTINATION lib/${PROJECT_NAME} ) if(BUILD_TESTING) diff --git a/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/base_driver.hpp b/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/base_driver.hpp deleted file mode 100644 index 6a2aedd..0000000 --- a/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/base_driver.hpp +++ /dev/null @@ -1,71 +0,0 @@ -// Copyright 2024 Trossen Robotics -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -#ifndef INTERBOTIX_SLATE_DRIVER__BASE_DRIVER_HPP_ -#define INTERBOTIX_SLATE_DRIVER__BASE_DRIVER_HPP_ - -#include - -#include "interbotix_slate_driver/serial_driver.hpp" - - -namespace base_driver -{ - -typedef struct -{ - float cmd_vel_x; - float cmd_vel_y; - float cmd_vel_z; - uint32_t light_state; - - uint32_t system_state; - uint32_t charge; - float voltage; - float current; - float vel_x; - float vel_y; - float vel_z; - float odom_x; - float odom_y; - float odom_z; - uint32_t cmd; - uint32_t io; - uint32_t err; -} ChassisData; - -bool chassisInit(std::string &dev); -bool getVersion(char *data); -bool setText(const char *text); -bool updateChassisInfo(ChassisData *data); -bool setSysCmd(uint32_t cmd); -bool setIo(uint32_t io); - -} // namespace base_driver - -#endif // INTERBOTIX_SLATE_DRIVER__BASE_DRIVER_HPP_ diff --git a/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/serial_driver.hpp b/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/serial_driver.hpp deleted file mode 100644 index 3171d32..0000000 --- a/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/serial_driver.hpp +++ /dev/null @@ -1,84 +0,0 @@ -// Copyright 2024 Trossen Robotics -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -#ifndef INTERBOTIX_SLATE_DRIVER__SERIAL_DRIVER_HPP_ -#define INTERBOTIX_SLATE_DRIVER__SERIAL_DRIVER_HPP_ - -#include - -#include -#include - -typedef enum -{ - SYS_INIT = 0x00, - SYS_NORMAL, - SYS_REMOTE, - SYS_ESTOP, - SYS_CALIB, - SYS_TEST, - SYS_CHARGING, - - SYS_ERR = 0x10, - SYS_ERR_ID, - SYS_ERR_COM, - SYS_ERR_ENC, - SYS_ERR_COLLISION, - SYS_ERR_LOW_VOLTAGE, - SYS_ERR_OVER_VOLTAGE, - SYS_ERR_OVER_CURRENT, - SYS_ERR_OVER_TEMP, - - SYS_STATE_LEN, -} SystemState; - -class SerialDriver -{ - public: - ~SerialDriver(); - bool init(std::string& port, uint8_t id, int baud = B115200); - - bool getVersion(char* version); - bool setText(const char* text); - - int readHoldingRegs(uint16_t addr, uint16_t cnt, uint16_t* data); - int writeHoldingRegs(uint16_t addr, uint16_t cnt, uint16_t* data); - int readWriteHoldingRegs(uint16_t raddr, uint16_t rcnt, uint16_t* rdata, - uint16_t waddr, uint16_t wcnt, uint16_t* wdata); - - int send(const void* data, int len, int timeout = 0); - int recv(uint8_t* data, int maxlen, int timeout = 0); - - private: - void* m = nullptr; - int fd_ = -1; - fd_set read_set_; - std::mutex lock; -}; - -#endif // INTERBOTIX_SLATE_DRIVER__SERIAL_DRIVER_HPP_ diff --git a/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/slate_base.hpp b/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/slate_base.hpp index 8cd3353..cf9e552 100644 --- a/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/slate_base.hpp +++ b/interbotix_ros_slate/interbotix_slate_driver/include/interbotix_slate_driver/slate_base.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Trossen Robotics +// Copyright 2025 Trossen Robotics // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -31,6 +31,7 @@ #include #include +#include #include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -44,15 +45,10 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/transform_broadcaster.h" -#include "interbotix_slate_driver/base_driver.hpp" -#include "interbotix_slate_driver/serial_driver.hpp" +#include "trossen_slate/trossen_slate.hpp" namespace slate_base { - -#define CMD_TIME_OUT 300 // ms -#define PORT "chassis" - using geometry_msgs::msg::Quaternion; using geometry_msgs::msg::TransformStamped; using geometry_msgs::msg::Twist; @@ -62,7 +58,9 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::BatteryState; using std_srvs::srv::SetBool; -class SlateBase : public rclcpp::Node +class SlateBase + : public trossen_slate::TrossenSlate, + public rclcpp::Node { public: /** @@ -71,21 +69,45 @@ class SlateBase : public rclcpp::Node */ explicit SlateBase(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - /// @brief Destructor for the SlateBase + /// @brief Destructor for SlateBase ~SlateBase() {} /// @brief Process velocity commands and update robot state void update(); private: - // Linear velocity command - float cmd_vel_x_; + // Update counter used to update the battery status less frequently + int cnt_; - // Angular velocity command - float cmd_vel_z_; + // Frequency of the update timer + int frequency_; - // Time last velocity command was received - rclcpp::Time cmd_vel_time_last_update_; + // Array containing x and y translation in meters and rotation in radians + float pose_[3]; + + // Whether or not to publish TF from base_link->odom + bool publish_tf_; + + // Whether or not we have received our first odometry update + bool is_first_odom_; + + // Base light state - see interbotix_slate_msgs/srv/SetLightState for details + uint32_t light_state_ = 0; + + // Base command bytes containing data about charging and motor torque enabling + uint32_t sys_cmd_ = 0; + + // Stored data of the base + base_driver::ChassisData data_; + + // Name of odom frame + std::string odom_frame_name_; + + // Name of base frame + std::string base_frame_name_; + + // Time of the current update + rclcpp::Time current_time_; // Odometry publisher rclcpp::Publisher::SharedPtr pub_odom_; @@ -108,72 +130,18 @@ class SlateBase : public rclcpp::Node // Set light state service server rclcpp::Service::SharedPtr srv_set_light_state_; - // Name of odom frame - std::string odom_frame_name_; - - // Name of base frame - std::string base_frame_name_; - - // Update counter used to only update some values less frequently - int cnt_; - - // Odometry translation in the x-direction in meters - float x_; - - // Odometry translation in the y-direction in meters - float y_; - - // Odometry rotation about the z-axis in radians - float theta_; - - // Odometry forward velocity in meters per second - float x_vel_; - - // Odometry rotational velocity about the z-axis in radians per second - float z_omega_; - - // Whether or not we have received our first odometry update - bool is_first_odom_; - - // Array containing x and y translation in meters and rotation in radians - float pose_[3]; - - // Current of the right motor in Amps - float right_motor_c_; - - // Current of the left motor in Amps - float left_motor_c_; - - // The system state of the base - SystemState chassis_state_; - - // Whether or not to publish TF from base_link->odom - bool publish_tf_; - - // Max linear velocity in the x-direction in meters per second - float max_vel_x_ = 1.0; - - // Max angular velocity about the z-axis in radians per second - float max_vel_z_ = 1.0; - - // Base command bytes containing data about charging and motor torque enabling - uint32_t sys_cmd_ = 0; - - // Base light state - see interbotix_slate_msgs/srv/SetLightState for details - uint32_t light_state_ = 0; + // Timer used to update the base state + rclcpp::TimerBase::SharedPtr timer_; // If publish_tf_ is true, this is the broadcaster used to publish the odom->base_link TF tf2_ros::TransformBroadcaster tf_broadcaster_odom_; - // Time of the current update - rclcpp::Time current_time_; - - // Time of the last update - rclcpp::Time last_time_; - // Timeout for base velocity rclcpp::Duration cmd_vel_timeout_; + // Time last velocity command was received + rclcpp::Time cmd_vel_time_last_update_; + /** * @brief Process incoming Twist command message * @param msg Incoming Twist command message diff --git a/interbotix_ros_slate/interbotix_slate_driver/lib/aarch64/libchassis_driver.so b/interbotix_ros_slate/interbotix_slate_driver/lib/aarch64/libchassis_driver.so deleted file mode 100644 index 30779ed..0000000 Binary files a/interbotix_ros_slate/interbotix_slate_driver/lib/aarch64/libchassis_driver.so and /dev/null differ diff --git a/interbotix_ros_slate/interbotix_slate_driver/lib/x86_64/libchassis_driver.so b/interbotix_ros_slate/interbotix_slate_driver/lib/x86_64/libchassis_driver.so deleted file mode 100644 index b2f592b..0000000 Binary files a/interbotix_ros_slate/interbotix_slate_driver/lib/x86_64/libchassis_driver.so and /dev/null differ diff --git a/interbotix_ros_slate/interbotix_slate_driver/package.xml b/interbotix_ros_slate/interbotix_slate_driver/package.xml index 7214c39..06c06f6 100644 --- a/interbotix_ros_slate/interbotix_slate_driver/package.xml +++ b/interbotix_ros_slate/interbotix_slate_driver/package.xml @@ -20,12 +20,7 @@ std_srvs tf2_geometry_msgs tf2_ros - - rosidl_default_generators - - rosidl_default_runtime - - rosidl_interface_packages + trossen_slate ament_cmake diff --git a/interbotix_ros_slate/interbotix_slate_driver/src/base_driver.cpp b/interbotix_ros_slate/interbotix_slate_driver/src/base_driver.cpp deleted file mode 100644 index 9111648..0000000 --- a/interbotix_ros_slate/interbotix_slate_driver/src/base_driver.cpp +++ /dev/null @@ -1,74 +0,0 @@ -// Copyright 2024 Trossen Robotics -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the copyright holder nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -#include "rclcpp/rclcpp.hpp" - -#include "interbotix_slate_driver/base_driver.hpp" -#include "interbotix_slate_driver/serial_driver.hpp" - -namespace base_driver -{ - -#define MAX_TIMEOUT_CNT 50 - -uint32_t err_cnt = 0; -SerialDriver driver; - -bool chassisInit(std::string &dev) { return driver.init(dev, 1, B115200); } - -bool check(int ret) -{ - err_cnt += (ret == 0 ? 0 : 1); - if (err_cnt > MAX_TIMEOUT_CNT) - { - std::string dev; - err_cnt = 0; - } - return !ret; -} - -bool getVersion(char *data) { return driver.getVersion(data); } -bool setText(const char *text) { return driver.setText(text); } - -bool updateChassisInfo(ChassisData *data) -{ - return check(driver.readWriteHoldingRegs(0x00, 26, (uint16_t *)&(data->system_state), - 0x40, 8, (uint16_t *)data)); -} - -bool setSysCmd(uint32_t cmd) -{ - return check(driver.writeHoldingRegs(0x14, 2, (uint16_t *)&cmd)); -} - -bool setIo(uint32_t io) -{ - return check(driver.writeHoldingRegs(0x16, 2, (uint16_t *)&io)); -} - -} // namespace base_driver diff --git a/interbotix_ros_slate/interbotix_slate_driver/src/slate_base.cpp b/interbotix_ros_slate/interbotix_slate_driver/src/slate_base.cpp index 90da2f2..64275a0 100644 --- a/interbotix_ros_slate/interbotix_slate_driver/src/slate_base.cpp +++ b/interbotix_ros_slate/interbotix_slate_driver/src/slate_base.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Trossen Robotics +// Copyright 2025 Trossen Robotics // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -26,28 +26,28 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include "interbotix_slate_driver/slate_base.hpp" - #include +#include + +#include "interbotix_slate_driver/slate_base.hpp" namespace slate_base { SlateBase::SlateBase(const rclcpp::NodeOptions & options) -: rclcpp::Node("slate_base", "", options), cmd_vel_x_(0.0), cmd_vel_z_(0.0), cnt_(0), x_(0.0), - y_(0.0), theta_(0.0), x_vel_(0.0), z_omega_(0.0), is_first_odom_(true), pose_{0}, - right_motor_c_(0.0), left_motor_c_(0.0), chassis_state_(SystemState::SYS_INIT), - publish_tf_(false), max_vel_x_(1.0), max_vel_z_(1.0), current_time_(get_clock()->now()), - last_time_(get_clock()->now()), tf_broadcaster_odom_(this), - cmd_vel_time_last_update_(get_clock()->now()), +: trossen_slate::TrossenSlate(), rclcpp::Node("slate_base", options), cnt_(0), pose_{0}, + publish_tf_(false), is_first_odom_(0), current_time_(get_clock()->now()), tf_broadcaster_odom_( + this), cmd_vel_time_last_update_(get_clock()->now()), cmd_vel_timeout_(rclcpp::Duration(std::chrono::milliseconds(CMD_TIME_OUT))) { using std::placeholders::_1, std::placeholders::_2, std::placeholders::_3; + declare_parameter("update_frequency", 20); declare_parameter("publish_tf", false); declare_parameter("odom_frame_name", "odom"); declare_parameter("base_frame_name", "base_link"); + get_parameter("update_frequency", frequency_); get_parameter("publish_tf", publish_tf_); get_parameter("odom_frame_name", odom_frame_name_); get_parameter("base_frame_name", base_frame_name_); @@ -76,78 +76,59 @@ SlateBase::SlateBase(const rclcpp::NodeOptions & options) "set_light_state", std::bind(&SlateBase::set_light_state_callback, this, _1, _2, _3)); - std::string dev; - if (!base_driver::chassisInit(dev)) { - RCLCPP_ERROR(get_logger(), "Failed to initialize base port."); - ::exit(EXIT_FAILURE); - } - RCLCPP_INFO(get_logger(), "Initalized base at port '%s'.", dev.c_str()); - char version[32] = {0}; - if (base_driver::getVersion(version)) { - RCLCPP_INFO(get_logger(), "Base version: %s", version); + auto duration = std::chrono::milliseconds(1000 / frequency_); + timer_ = create_wall_timer(duration, std::bind(&SlateBase::update, this)); + + std::string result; + if (!init_base(result)) { + RCLCPP_FATAL(get_logger(), result.c_str()); + } else { + RCLCPP_INFO(get_logger(), result.c_str()); } } void SlateBase::update() { - rclcpp::spin_some(get_node_base_interface()); current_time_ = get_clock()->now(); - // time out velocity commands + // Time out velocity commands if (current_time_ - cmd_vel_time_last_update_ > cmd_vel_timeout_) { - cmd_vel_x_ = 0.0f; - cmd_vel_z_ = 0.0f; + data_.cmd_vel_x = 0.0f; + data_.cmd_vel_z = 0.0f; } - // limit velocity commands - cmd_vel_x_ = std::min(max_vel_x_, std::max(-max_vel_x_, cmd_vel_x_)); - cmd_vel_z_ = std::min(max_vel_z_, std::max(-max_vel_z_, cmd_vel_z_)); - - // initialize chassis data and use it to update the driver - base_driver::ChassisData data = { - .cmd_vel_x=cmd_vel_x_, - .cmd_vel_y=0.0, - .cmd_vel_z=cmd_vel_z_, - .light_state=light_state_, - .system_state=0}; - - if (!base_driver::updateChassisInfo(&data)) { + // Update base state + if (!base_driver::updateChassisInfo(&data_)) { return; } - // extract and update base system command bytes - sys_cmd_ = data.cmd; - - // update battery state every 10 iterations + // Update battery state every 10 iterations cnt_++; auto battery_state = BatteryState(); if (cnt_ % 10 == 0) { battery_state.header.stamp = current_time_; - battery_state.voltage = data.voltage; - battery_state.current = data.current; - battery_state.percentage = data.charge; - battery_state.power_supply_status = data.system_state; + battery_state.voltage = data_.voltage; + battery_state.temperature = std::numeric_limits::quiet_NaN(); + battery_state.current = data_.current; + battery_state.charge = std::numeric_limits::quiet_NaN(); + battery_state.capacity = std::numeric_limits::quiet_NaN(); + battery_state.design_capacity = std::numeric_limits::quiet_NaN(); + battery_state.percentage = data_.charge; + battery_state.present = true; pub_battery_state_->publish(battery_state); } - // update odometry values - x_vel_ = data.vel_x; - z_omega_ = data.vel_z; - - x_ = data.odom_x; - y_ = data.odom_y; - theta_ = data.odom_z; - + // Set initial pose if (is_first_odom_) { - pose_[0] = x_; - pose_[1] = y_; - pose_[2] = theta_; + pose_[0] = data_.odom_x; + pose_[1] = data_.odom_y; + pose_[2] = data_.odom_z; is_first_odom_ = false; } - // create transform + // Create transform tf2::Quaternion q; - q.setRPY(0, 0, wrap_angle(theta_ - pose_[2])); + q.setRPY(0, 0, wrap_angle(data_.odom_z - pose_[2])); auto odom_quat = tf2::toMsg(q); auto odom_trans = TransformStamped(); odom_trans.header.stamp = current_time_; @@ -155,44 +136,43 @@ void SlateBase::update() odom_trans.child_frame_id = base_frame_name_; odom_trans.transform.translation.x = - cos(-pose_[2]) * (x_ - pose_[0]) - sin(-pose_[2]) * (y_ - pose_[1]); + cos(-pose_[2]) * (data_.odom_x - pose_[0]) - sin(-pose_[2]) * (data_.odom_y - pose_[1]); odom_trans.transform.translation.y = - sin(-pose_[2]) * (x_ - pose_[0]) + cos(-pose_[2]) * (y_ - pose_[1]); + sin(-pose_[2]) * (data_.odom_x - pose_[0]) + cos(-pose_[2]) * (data_.odom_y - -pose_[1]); odom_trans.transform.translation.z = 0.0; odom_trans.transform.translation.z = 0.0; odom_trans.transform.rotation = odom_quat; - // send the transform + // Send the transform if (publish_tf_) { tf_broadcaster_odom_.sendTransform(odom_trans); } - // publish odometry + // Publish odometry auto odom = Odometry(); odom.header.stamp = current_time_; odom.header.frame_id = odom_frame_name_; - // set position + // Set pose odom.pose.pose.position.x = odom_trans.transform.translation.x; odom.pose.pose.position.y = odom_trans.transform.translation.y; odom.pose.pose.position.z = 0.0; odom.pose.pose.orientation = odom_quat; - odom.pose.covariance[0] = (data.system_state == SystemState::SYS_ESTOP) ? -1 : 1; + odom.pose.covariance[0] = (data_.system_state == SystemState::SYS_ESTOP) ? -1 : 1; - // set velocity + // Set cmd velocity odom.child_frame_id = base_frame_name_; - odom.twist.twist.linear.x = x_vel_; + odom.twist.twist.linear.x = data_.vel_x; odom.twist.twist.linear.y = 0; - odom.twist.twist.angular.z = z_omega_; + odom.twist.twist.angular.z = data_.vel_z; pub_odom_->publish(odom); - last_time_ = current_time_; } void SlateBase::cmd_vel_callback(const Twist::SharedPtr msg) { - cmd_vel_x_ = msg->linear.x; - cmd_vel_z_ = msg->angular.z; + data_.cmd_vel_x = msg->linear.x; + data_.cmd_vel_z = msg->angular.z; cmd_vel_time_last_update_ = get_clock()->now(); } @@ -201,10 +181,9 @@ bool SlateBase::set_text_callback( const std::shared_ptr req, const std::shared_ptr res) { + res->success = set_text(req->data); res->message = "Set base screen text to: '" + req->data + "'."; - base_driver::setText(req->data.c_str()); RCLCPP_INFO(get_logger(), res->message.c_str()); - res->success = true; return true; } @@ -213,17 +192,10 @@ bool SlateBase::motor_torque_status_callback( const std::shared_ptr req, const std::shared_ptr res) { - req->data ? sys_cmd_ &= ~(1): sys_cmd_ |= 1; - res->success = base_driver::setSysCmd(sys_cmd_); - - std::string enabled_disabled = req->data ? "enable" : "disable"; - if (res->success) { - res->message = "Successfully " + enabled_disabled + "d motor torque."; - RCLCPP_INFO(get_logger(), res->message.c_str()); - } else { - res->message = "Failed to " + enabled_disabled + " motor torque."; - RCLCPP_ERROR(get_logger(), res->message.c_str()); - } + std::string result; + res->success = enable_motor_torque(req->data, result); + res->message = result; + RCLCPP_INFO(get_logger(), res->message.c_str()); return res->success; } @@ -232,17 +204,10 @@ bool SlateBase::enable_charging_callback( const std::shared_ptr req, const std::shared_ptr res) { - req->data ? sys_cmd_ |= 2 : sys_cmd_ &= ~(2); - res->success = base_driver::setSysCmd(sys_cmd_); - - std::string enabled_disabled = req->data ? "enable" : "disable"; - if (res->success) { - res->message = "Successfully " + enabled_disabled + "d charging."; - RCLCPP_INFO(get_logger(), res->message.c_str()); - } else { - res->message = "Failed to " + enabled_disabled + " charging."; - RCLCPP_ERROR(get_logger(), res->message.c_str()); - } + std::string result; + res->success = enable_charging(req->data, result); + res->message = result; + RCLCPP_INFO(get_logger(), res->message.c_str()); return res->success; } @@ -251,10 +216,11 @@ bool SlateBase::set_light_state_callback( const std::shared_ptr req, const std::shared_ptr res) { + LightState light_state = static_cast(req->light_state); + res->success = set_light_state(light_state); res->message = "Set light state to: '" + std::to_string(req->light_state) + "'."; - light_state_ = req->light_state; + data_.light_state = req->light_state; RCLCPP_INFO(get_logger(), res->message.c_str()); - res->success = true; return true; } diff --git a/interbotix_ros_slate/interbotix_slate_driver/src/slate_base_node.cpp b/interbotix_ros_slate/interbotix_slate_driver/src/slate_base_node.cpp index 3000759..8e5fb45 100644 --- a/interbotix_ros_slate/interbotix_slate_driver/src/slate_base_node.cpp +++ b/interbotix_ros_slate/interbotix_slate_driver/src/slate_base_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Trossen Robotics +// Copyright 2025 Trossen Robotics // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -33,11 +33,7 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); - - auto r = rclcpp::Rate(20); - while (rclcpp::ok()) { - node->update(); - r.sleep(); - } + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; } diff --git a/interbotix_ros_slate/trossen_slate b/interbotix_ros_slate/trossen_slate new file mode 160000 index 0000000..672f34c --- /dev/null +++ b/interbotix_ros_slate/trossen_slate @@ -0,0 +1 @@ +Subproject commit 672f34caef27aeeb9ea10b68df544bba3290acd0