diff --git a/src/ros2aria/CMakeLists.txt b/src/ros2aria/CMakeLists.txt index 36928ce..57ec68f 100644 --- a/src/ros2aria/CMakeLists.txt +++ b/src/ros2aria/CMakeLists.txt @@ -38,7 +38,8 @@ add_executable(ros2aria src/pose.cpp src/gripper.cpp src/wheels.cpp - src/clutch.cpp) + src/clutch.cpp + src/state.cpp) ament_target_dependencies(ros2aria rclcpp) ament_target_dependencies(ros2aria std_srvs) diff --git a/src/ros2aria/src/publish.cpp b/src/ros2aria/src/publish.cpp index 3b8353d..80f5d19 100644 --- a/src/ros2aria/src/publish.cpp +++ b/src/ros2aria/src/publish.cpp @@ -14,4 +14,6 @@ void Ros2Aria::publish() sensor_msgs::msg::JointState wheels = handleWheels(t); publishWheels(wheels); + + publishState(t); } \ No newline at end of file diff --git a/src/ros2aria/src/ros2aria.cpp b/src/ros2aria/src/ros2aria.cpp index bfccbc9..26ed9dd 100644 --- a/src/ros2aria/src/ros2aria.cpp +++ b/src/ros2aria/src/ros2aria.cpp @@ -22,6 +22,9 @@ Ros2Aria::Ros2Aria() wheels_pub_ = this->create_publisher("wheels", 1000); + battery_recharge_state_pub_ = this->create_publisher("battery_recharge_state", 10); // TODO: original rosaria latches this - what does that mean and how does it relate to ros2? + battery_state_of_charge_pub_ = this->create_publisher("battery_state_of_charge", 10); + // services stop_service_ = this->create_service("stop", std::bind(&Ros2Aria::stop, this, _1, _2)); gripper_open_service_ = this->create_service("gripper_open", std::bind(&Ros2Aria::gripper_open_callback, this, _1, _2)); diff --git a/src/ros2aria/src/ros2aria.hpp b/src/ros2aria/src/ros2aria.hpp index 9e9aa1f..80efaa1 100644 --- a/src/ros2aria/src/ros2aria.hpp +++ b/src/ros2aria/src/ros2aria.hpp @@ -8,6 +8,8 @@ #include "nav_msgs/msg/odometry.hpp" #include "std_srvs/srv/empty.hpp" #include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/int8.hpp" +#include "std_msgs/msg/float32.hpp" #include "./raiibot.cpp" @@ -44,6 +46,10 @@ private: rclcpp::Publisher::SharedPtr wheels_pub_; void publishWheels(sensor_msgs::msg::JointState wheels); + rclcpp::Publisher::SharedPtr battery_recharge_state_pub_; + rclcpp::Publisher::SharedPtr battery_state_of_charge_pub_; + void publishState(rclcpp::Time stamp); + // subscribers rclcpp::Subscription::SharedPtr cmd_vel_sub_; void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg); diff --git a/src/ros2aria/src/state.cpp b/src/ros2aria/src/state.cpp new file mode 100644 index 0000000..9001fa5 --- /dev/null +++ b/src/ros2aria/src/state.cpp @@ -0,0 +1,23 @@ +#include "ros2aria.hpp" + +void Ros2Aria::publishState(rclcpp::Time stamp) +{ + auto r = this->robot->getRobot(); + + // TODO: original rosaria only publishes it when it changes. + if (this->battery_recharge_state_pub_->get_subscription_count() > 0) + { + char s = r->getChargeState(); + std_msgs::msg::Int8 recharge_state; + recharge_state.data = s; + this->battery_recharge_state_pub_->publish(recharge_state); + } + + // TODO: our robots don't have this, cannot test + if (r->haveStateOfCharge() && this->battery_state_of_charge_pub_->get_subscription_count() > 0) + { + std_msgs::msg::Float32 soc; + soc.data = r->getStateOfCharge() / 100.0; + this->battery_state_of_charge_pub_->publish(soc); + } +} \ No newline at end of file