diff --git a/scout_base/include/scout_base/scout_messenger.hpp b/scout_base/include/scout_base/scout_messenger.hpp index fcef95a..4d84738 100644 --- a/scout_base/include/scout_base/scout_messenger.hpp +++ b/scout_base/include/scout_base/scout_messenger.hpp @@ -22,263 +22,281 @@ #include "scout_msgs/msg/scout_status.hpp" #include "scout_msgs/msg/scout_light_cmd.hpp" +#include "scout_msgs/msg/scout_bms_status.hpp" #include "ugv_sdk/mobile_robot/scout_robot.hpp" namespace westonrobot { template class ScoutMessenger { - public: + public: ScoutMessenger(std::shared_ptr scout, rclcpp::Node *node) - : scout_(scout), node_(node) {} + : scout_(scout), node_(node) {} - void SetOdometryFrame(std::string frame) { odom_frame_ = frame; } - void SetBaseFrame(std::string frame) { base_frame_ = frame; } - void SetOdometryTopicName(std::string name) { odom_topic_name_ = name; } + void SetOdometryFrame(std::string frame) { odom_frame_ = frame; } + void SetBaseFrame(std::string frame) { base_frame_ = frame; } + void SetOdometryTopicName(std::string name) { odom_topic_name_ = name; } - void SetSimulationMode(int loop_rate) { - simulated_robot_ = true; - sim_control_rate_ = loop_rate; - } + void SetSimulationMode(int loop_rate) { + simulated_robot_ = true; + sim_control_rate_ = loop_rate; + } - void SetupSubscription() { - // odometry publisher + void SetupSubscription() { + // odometry publisher odom_pub_ = node_->create_publisher(odom_topic_name_, 50); - status_pub_ = node_->create_publisher( - "/scout_status", 10); + status_pub_ = node_->create_publisher( + "/scout_status", 10); + + bms_status_pub_ = node_->create_publisher( + "/scout_bms_status", 10); - // cmd subscriber + // cmd subscriber motion_cmd_sub_ = node_->create_subscription( - "/cmd_vel", 5, - std::bind(&ScoutMessenger::TwistCmdCallback, this, - std::placeholders::_1)); + "/cmd_vel", 5, + std::bind(&ScoutMessenger::TwistCmdCallback, this, + std::placeholders::_1)); light_cmd_sub_ = node_->create_subscription( - "/light_control", 5, - std::bind(&ScoutMessenger::LightCmdCallback, this, - std::placeholders::_1)); + "/light_control", 5, + std::bind(&ScoutMessenger::LightCmdCallback, this, + std::placeholders::_1)); tf_broadcaster_ = std::make_shared(node_); - } - - void PublishStateToROS() { - current_time_ = node_->get_clock()->now(); - - static bool init_run = true; - if (init_run) { - last_time_ = current_time_; - init_run = false; - return; } - double dt = (current_time_ - last_time_).seconds(); - auto state = scout_->GetRobotState(); + void PublishStateToROS() { + current_time_ = node_->get_clock()->now(); - // publish scout state message - scout_msgs::msg::ScoutStatus status_msg; + static bool init_run = true; + if (init_run) { + last_time_ = current_time_; + init_run = false; + return; + } + double dt = (current_time_ - last_time_).seconds(); - status_msg.header.stamp = current_time_; + auto state = scout_->GetRobotState(); - status_msg.linear_velocity = state.motion_state.linear_velocity; - status_msg.angular_velocity = state.motion_state.angular_velocity; + // publish scout state message + scout_msgs::msg::ScoutStatus status_msg; - status_msg.vehicle_state = state.system_state.vehicle_state; - status_msg.control_mode = state.system_state.control_mode; - status_msg.error_code = state.system_state.error_code; - status_msg.battery_voltage = state.system_state.battery_voltage; + status_msg.header.stamp = current_time_; - auto actuator = scout_->GetActuatorState(); + status_msg.linear_velocity = state.motion_state.linear_velocity; + status_msg.angular_velocity = state.motion_state.angular_velocity; - for (int i = 0; i < 4; ++i) { - // actuator_hs_state - uint8_t motor_id = actuator.actuator_hs_state[i].motor_id; + status_msg.vehicle_state = state.system_state.vehicle_state; + status_msg.control_mode = state.system_state.control_mode; + status_msg.error_code = state.system_state.error_code; + status_msg.battery_voltage = state.system_state.battery_voltage; - status_msg.actuator_states[motor_id].rpm = - actuator.actuator_hs_state[i].rpm; - status_msg.actuator_states[motor_id].current = - actuator.actuator_hs_state[i].current; - status_msg.actuator_states[motor_id].pulse_count = - actuator.actuator_hs_state[i].pulse_count; + auto actuator = scout_->GetActuatorState(); - // actuator_ls_state - motor_id = actuator.actuator_ls_state[i].motor_id; + for (int i = 0; i < 4; ++i) { + // actuator_hs_state + uint8_t motor_id = actuator.actuator_hs_state[i].motor_id; - status_msg.actuator_states[motor_id].driver_voltage = - actuator.actuator_ls_state[i].driver_voltage; - status_msg.actuator_states[motor_id].driver_temperature = - actuator.actuator_ls_state[i].driver_temp; - status_msg.actuator_states[motor_id].motor_temperature = - actuator.actuator_ls_state[i].motor_temp; - status_msg.actuator_states[motor_id].driver_state = - actuator.actuator_ls_state[i].driver_state; - } + status_msg.actuator_states[motor_id].rpm = + actuator.actuator_hs_state[i].rpm; + status_msg.actuator_states[motor_id].current = + actuator.actuator_hs_state[i].current; + status_msg.actuator_states[motor_id].pulse_count = + actuator.actuator_hs_state[i].pulse_count; - status_msg.light_control_enabled = state.light_state.enable_cmd_ctrl; - status_msg.front_light_state.mode = state.light_state.front_light.mode; - status_msg.front_light_state.custom_value = - state.light_state.front_light.custom_value; - status_msg.rear_light_state.mode = state.light_state.rear_light.mode; - status_msg.rear_light_state.custom_value = - state.light_state.rear_light.custom_value; - status_pub_->publish(status_msg); + // actuator_ls_state + motor_id = actuator.actuator_ls_state[i].motor_id; - // publish odometry and tf - PublishOdometryToROS(state.motion_state, dt); + status_msg.actuator_states[motor_id].driver_voltage = + actuator.actuator_ls_state[i].driver_voltage; + status_msg.actuator_states[motor_id].driver_temperature = + actuator.actuator_ls_state[i].driver_temp; + status_msg.actuator_states[motor_id].motor_temperature = + actuator.actuator_ls_state[i].motor_temp; + status_msg.actuator_states[motor_id].driver_state = + actuator.actuator_ls_state[i].driver_state; + } - // record time for next integration - last_time_ = current_time_; - } + status_msg.light_control_enabled = state.light_state.enable_cmd_ctrl; + status_msg.front_light_state.mode = state.light_state.front_light.mode; + status_msg.front_light_state.custom_value = + state.light_state.front_light.custom_value; + status_msg.rear_light_state.mode = state.light_state.rear_light.mode; + status_msg.rear_light_state.custom_value = + state.light_state.rear_light.custom_value; + status_pub_->publish(status_msg); + + // publish odometry and tf + PublishOdometryToROS(state.motion_state, dt); + + // publish BMS data + auto bms_status = scout_->GetCommonSensorState(); + + auto bms_status_msg = scout_msgs::msg::ScoutBmsStatus(); + bms_status_msg.soc = bms_status.bms_basic_state.battery_soc; + bms_status_msg.soh = bms_status.bms_basic_state.battery_soh; + bms_status_msg.battery_voltage = bms_status.bms_basic_state.voltage; + bms_status_msg.battery_current = bms_status.bms_basic_state.current; + bms_status_msg.battery_temperature = + bms_status.bms_basic_state.temperature; + + bms_status_pub_->publish(bms_status_msg); + + // record time for next integration + last_time_ = current_time_; + } - private: - std::shared_ptr scout_; - rclcpp::Node *node_; + private: + std::shared_ptr scout_; + rclcpp::Node *node_; - std::string odom_frame_; - std::string base_frame_; - std::string odom_topic_name_; + std::string odom_frame_; + std::string base_frame_; + std::string odom_topic_name_; - bool simulated_robot_ = false; - int sim_control_rate_ = 50; + bool simulated_robot_ = false; + int sim_control_rate_ = 50; - std::mutex twist_mutex_; - geometry_msgs::msg::Twist current_twist_; + std::mutex twist_mutex_; + geometry_msgs::msg::Twist current_twist_; - rclcpp::Publisher::SharedPtr odom_pub_; - rclcpp::Publisher::SharedPtr status_pub_; + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher::SharedPtr status_pub_; + rclcpp::Publisher::SharedPtr bms_status_pub_; - rclcpp::Subscription::SharedPtr motion_cmd_sub_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr motion_cmd_sub_; + rclcpp::Subscription::SharedPtr light_cmd_sub_; - std::shared_ptr tf_broadcaster_; + std::shared_ptr tf_broadcaster_; - // speed variables - double position_x_ = 0.0; - double position_y_ = 0.0; - double theta_ = 0.0; + // speed variables + double position_x_ = 0.0; + double position_y_ = 0.0; + double theta_ = 0.0; - rclcpp::Time last_time_; - rclcpp::Time current_time_; + rclcpp::Time last_time_; + rclcpp::Time current_time_; - void TwistCmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { - if (!simulated_robot_) { - SetScoutMotionCommand(scout_, msg); - } else { - std::lock_guard guard(twist_mutex_); - current_twist_ = *msg.get(); + void TwistCmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { + if (!simulated_robot_) { + SetScoutMotionCommand(scout_, msg); + } else { + std::lock_guard guard(twist_mutex_); + current_twist_ = *msg.get(); + } + // ROS_INFO("Cmd received:%f, %f", msg->linear.x, msg->angular.z); } - // ROS_INFO("Cmd received:%f, %f", msg->linear.x, msg->angular.z); - } - template ::value, - bool> = true> + template ::value, + bool> = true> void SetScoutMotionCommand(std::shared_ptr base, const geometry_msgs::msg::Twist::SharedPtr &msg) { - base->SetMotionCommand(msg->linear.x, msg->angular.z); - } + base->SetMotionCommand(msg->linear.x, msg->angular.z); + } - template ::value, - bool> = true> + template ::value, + bool> = true> void SetScoutMotionCommand(std::shared_ptr base, const geometry_msgs::msg::Twist::SharedPtr &msg) { - base->SetMotionCommand(msg->linear.x, msg->angular.z, msg->linear.y); - } + base->SetMotionCommand(msg->linear.x, msg->angular.z, msg->linear.y); + } void LightCmdCallback(const scout_msgs::msg::ScoutLightCmd::SharedPtr msg) { - if (!simulated_robot_) { - if (msg->cmd_ctrl_allowed) { - AgxLightMode f_mode; - uint8_t f_value; - - switch (msg->front_mode) { - case scout_msgs::msg::ScoutLightCmd::LIGHT_CONST_OFF: { - f_mode = AgxLightMode::CONST_OFF; - break; - } - case scout_msgs::msg::ScoutLightCmd::LIGHT_CONST_ON: { - f_mode = AgxLightMode::CONST_ON; - break; - } - case scout_msgs::msg::ScoutLightCmd::LIGHT_BREATH: { - f_mode = AgxLightMode::BREATH; - break; - } - case scout_msgs::msg::ScoutLightCmd::LIGHT_CUSTOM: { - f_mode = AgxLightMode::CUSTOM; - f_value = msg->front_custom_value; - break; - } - } + if (!simulated_robot_) { + if (msg->cmd_ctrl_allowed) { + AgxLightMode f_mode; + uint8_t f_value; + + switch (msg->front_mode) { + case scout_msgs::msg::ScoutLightCmd::LIGHT_CONST_OFF: { + f_mode = AgxLightMode::CONST_OFF; + break; + } + case scout_msgs::msg::ScoutLightCmd::LIGHT_CONST_ON: { + f_mode = AgxLightMode::CONST_ON; + break; + } + case scout_msgs::msg::ScoutLightCmd::LIGHT_BREATH: { + f_mode = AgxLightMode::BREATH; + break; + } + case scout_msgs::msg::ScoutLightCmd::LIGHT_CUSTOM: { + f_mode = AgxLightMode::CUSTOM; + f_value = msg->front_custom_value; + break; + } + } scout_->SetLightCommand(f_mode, f_value, AgxLightMode::CONST_ON, 0); - } else { - scout_->DisableLightControl(); - } - } else { + } else { + scout_->DisableLightControl(); + } + } else { std::cout << "simulated robot received light control cmd" << std::endl; + } } - } - geometry_msgs::msg::Quaternion createQuaternionMsgFromYaw(double yaw) { - tf2::Quaternion q; - q.setRPY(0, 0, yaw); - return tf2::toMsg(q); - } + geometry_msgs::msg::Quaternion createQuaternionMsgFromYaw(double yaw) { + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + return tf2::toMsg(q); + } void PublishOdometryToROS(const MotionStateMessage &msg, double dt) { - // perform numerical integration to get an estimation of pose - double linear_speed = msg.linear_velocity; - double angular_speed = msg.angular_velocity; - double lateral_speed = 0; - - if (std::is_base_of::value) { - lateral_speed = msg.lateral_velocity; - } else { - lateral_speed = 0; - } + // perform numerical integration to get an estimation of pose + double linear_speed = msg.linear_velocity; + double angular_speed = msg.angular_velocity; + double lateral_speed = 0; + + if (std::is_base_of::value) { + lateral_speed = msg.lateral_velocity; + } else { + lateral_speed = 0; + } - double d_x = linear_speed * std::cos(theta_) * dt; - double d_y = (linear_speed * std::sin(theta_) + lateral_speed) * dt; - double d_theta = angular_speed * dt; + double d_x = linear_speed * std::cos(theta_) * dt; + double d_y = (linear_speed * std::sin(theta_) + lateral_speed) * dt; + double d_theta = angular_speed * dt; - position_x_ += d_x; - position_y_ += d_y; - theta_ += d_theta; + position_x_ += d_x; + position_y_ += d_y; + theta_ += d_theta; - geometry_msgs::msg::Quaternion odom_quat = - createQuaternionMsgFromYaw(theta_); + geometry_msgs::msg::Quaternion odom_quat = + createQuaternionMsgFromYaw(theta_); - // publish tf transformation - geometry_msgs::msg::TransformStamped tf_msg; - tf_msg.header.stamp = current_time_; - tf_msg.header.frame_id = odom_frame_; - tf_msg.child_frame_id = base_frame_; + // publish tf transformation + geometry_msgs::msg::TransformStamped tf_msg; + tf_msg.header.stamp = current_time_; + tf_msg.header.frame_id = odom_frame_; + tf_msg.child_frame_id = base_frame_; - tf_msg.transform.translation.x = position_x_; - tf_msg.transform.translation.y = position_y_; - tf_msg.transform.translation.z = 0.0; - tf_msg.transform.rotation = odom_quat; + tf_msg.transform.translation.x = position_x_; + tf_msg.transform.translation.y = position_y_; + tf_msg.transform.translation.z = 0.0; + tf_msg.transform.rotation = odom_quat; - tf_broadcaster_->sendTransform(tf_msg); + tf_broadcaster_->sendTransform(tf_msg); - // publish odometry and tf messages - nav_msgs::msg::Odometry odom_msg; - odom_msg.header.stamp = current_time_; - odom_msg.header.frame_id = odom_frame_; - odom_msg.child_frame_id = base_frame_; + // publish odometry and tf messages + nav_msgs::msg::Odometry odom_msg; + odom_msg.header.stamp = current_time_; + odom_msg.header.frame_id = odom_frame_; + odom_msg.child_frame_id = base_frame_; - odom_msg.pose.pose.position.x = position_x_; - odom_msg.pose.pose.position.y = position_y_; - odom_msg.pose.pose.position.z = 0.0; - odom_msg.pose.pose.orientation = odom_quat; + odom_msg.pose.pose.position.x = position_x_; + odom_msg.pose.pose.position.y = position_y_; + odom_msg.pose.pose.position.z = 0.0; + odom_msg.pose.pose.orientation = odom_quat; - odom_msg.twist.twist.linear.x = linear_speed; - odom_msg.twist.twist.linear.y = lateral_speed; - odom_msg.twist.twist.angular.z = angular_speed; + odom_msg.twist.twist.linear.x = linear_speed; + odom_msg.twist.twist.linear.y = lateral_speed; + odom_msg.twist.twist.angular.z = angular_speed; - odom_pub_->publish(odom_msg); - } + odom_pub_->publish(odom_msg); + } }; } // namespace westonrobot diff --git a/scout_msgs/CMakeLists.txt b/scout_msgs/CMakeLists.txt index 5f8d1c3..019a3ee 100644 --- a/scout_msgs/CMakeLists.txt +++ b/scout_msgs/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(std_msgs REQUIRED) set(msg_file "msg/ScoutActuatorState.msg" +"msg/ScoutBmsStatus.msg" "msg/ScoutLightCmd.msg" "msg/ScoutLightState.msg" "msg/ScoutRCState.msg" diff --git a/scout_msgs/msg/ScoutBmsStatus.msg b/scout_msgs/msg/ScoutBmsStatus.msg new file mode 100644 index 0000000..0e30d62 --- /dev/null +++ b/scout_msgs/msg/ScoutBmsStatus.msg @@ -0,0 +1,6 @@ +#BMS date +uint8 soc +uint8 soh +float32 battery_voltage +float32 battery_current +float32 battery_temperature \ No newline at end of file