diff --git a/scout_base/include/scout_base/scout_messenger.hpp b/scout_base/include/scout_base/scout_messenger.hpp index 45c932e..497ab87 100644 --- a/scout_base/include/scout_base/scout_messenger.hpp +++ b/scout_base/include/scout_base/scout_messenger.hpp @@ -1,9 +1,9 @@ -/* +/* * scout_messenger.hpp - * + * * Created on: Jun 14, 2019 10:24 - * Description: - * + * Description: + * * Copyright (c) 2019 Ruixiang Du (rdu) */ @@ -11,6 +11,7 @@ #define SCOUT_MESSENGER_HPP #include +#include #include #include @@ -18,57 +19,55 @@ #include #include "scout_msgs/ScoutLightCmd.h" -#include "ugv_sdk/scout_base.hpp" +#include "ugv_sdk/mobile_robot/scout_robot.hpp" -namespace westonrobot -{ -class ScoutROSMessenger -{ -public: - explicit ScoutROSMessenger(ros::NodeHandle *nh); - ScoutROSMessenger(ScoutBase *scout, ros::NodeHandle *nh); +namespace westonrobot { +class ScoutROSMessenger { + public: + explicit ScoutROSMessenger(ros::NodeHandle *nh); + ScoutROSMessenger(std::shared_ptr scout, ros::NodeHandle *nh); - 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; - void SetupSubscription(); + void SetupSubscription(); - void PublishStateToROS(); - void PublishSimStateToROS(double linear, double angular); + void PublishStateToROS(); + void PublishSimStateToROS(double linear, double angular); - void GetCurrentMotionCmdForSim(double &linear, double &angular); + void GetCurrentMotionCmdForSim(double &linear, double &angular); -private: - ScoutBase *scout_; - ros::NodeHandle *nh_; + private: + std::shared_ptr scout_; + ros::NodeHandle *nh_; - std::mutex twist_mutex_; - geometry_msgs::Twist current_twist_; + std::mutex twist_mutex_; + geometry_msgs::Twist current_twist_; - ros::Publisher odom_publisher_; - ros::Publisher status_publisher_; - ros::Subscriber motion_cmd_subscriber_; - ros::Subscriber light_cmd_subscriber_; - tf2_ros::TransformBroadcaster tf_broadcaster_; + ros::Publisher odom_publisher_; + ros::Publisher status_publisher_; + ros::Subscriber motion_cmd_subscriber_; + ros::Subscriber light_cmd_subscriber_; + tf2_ros::TransformBroadcaster tf_broadcaster_; - // speed variables - double linear_speed_ = 0.0; - double angular_speed_ = 0.0; - double position_x_ = 0.0; - double position_y_ = 0.0; - double theta_ = 0.0; + // speed variables + double linear_speed_ = 0.0; + double angular_speed_ = 0.0; + double position_x_ = 0.0; + double position_y_ = 0.0; + double theta_ = 0.0; - ros::Time last_time_; - ros::Time current_time_; + ros::Time last_time_; + ros::Time current_time_; - void TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg); - void LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg); - void PublishOdometryToROS(double linear, double angular, double dt); + void TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg); + void LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg); + void PublishOdometryToROS(double linear, double angular, double dt); }; -} // namespace westonrobot +} // namespace westonrobot #endif /* SCOUT_MESSENGER_HPP */ diff --git a/scout_base/include/scout_base/scout_params.hpp b/scout_base/include/scout_base/scout_params.hpp index 3c1f55d..0165362 100644 --- a/scout_base/include/scout_base/scout_params.hpp +++ b/scout_base/include/scout_base/scout_params.hpp @@ -1,33 +1,33 @@ -/* +/* * scout_params.hpp - * + * * Created on: Sep 27, 2019 15:08 - * Description: - * + * Description: + * * Copyright (c) 2020 Ruixiang Du (rdu) */ #ifndef SCOUT_PARAMS_HPP #define SCOUT_PARAMS_HPP -namespace westonrobot -{ - struct ScoutParams - { - /* Scout Parameters */ - static constexpr double max_steer_angle = 30.0; // in degree +namespace westonrobot { +struct ScoutParams { + /* Scout Parameters */ + static constexpr double max_steer_angle = 30.0; // in degree - static constexpr double track = 0.58306; // in meter (left & right wheel distance) - static constexpr double wheelbase = 0.498; // in meter (front & rear wheel distance) - static constexpr double wheel_radius = 0.165; // in meter + static constexpr double track = + 0.58306; // in meter (left & right wheel distance) + static constexpr double wheelbase = + 0.498; // in meter (front & rear wheel distance) + static constexpr double wheel_radius = 0.165; // in meter - // from user manual v1.2.8 P18 - // max linear velocity: 1.5 m/s - // max angular velocity: 0.7853 rad/s - static constexpr double max_linear_speed = 1.5; // in m/s - static constexpr double max_angular_speed = 0.7853; // in rad/s - static constexpr double max_speed_cmd = 10.0; // in rad/s - }; -} // namespace westonrobot + // from user manual v1.2.8 P18 + // max linear velocity: 1.5 m/s + // max angular velocity: 0.7853 rad/s + static constexpr double max_linear_speed = 1.5; // in m/s + static constexpr double max_angular_speed = 0.7853; // in rad/s + static constexpr double max_speed_cmd = 10.0; // in rad/s +}; +} // namespace westonrobot #endif /* SCOUT_PARAMS_HPP */ diff --git a/scout_base/src/scout_base_node.cpp b/scout_base/src/scout_base_node.cpp index 7827ff7..84710af 100644 --- a/scout_base/src/scout_base_node.cpp +++ b/scout_base/src/scout_base_node.cpp @@ -1,16 +1,17 @@ #include +#include #include #include #include #include -#include "ugv_sdk/scout_base.hpp" +#include "ugv_sdk/mobile_robot/scout_robot.hpp" #include "scout_base/scout_messenger.hpp" using namespace westonrobot; -std::shared_ptr robot; +std::shared_ptr robot; bool keep_run = true; void DetachRobot(int signal) { keep_run = false; } @@ -23,8 +24,8 @@ int main(int argc, char **argv) { std::signal(SIGINT, DetachRobot); // instantiate a robot object - robot = std::make_shared(); - ScoutROSMessenger messenger(robot.get(), &node); + robot = std::make_shared(); + ScoutROSMessenger messenger(robot, &node); // fetch parameters before connecting to robot std::string port_name; @@ -45,7 +46,7 @@ int main(int argc, char **argv) { robot->Connect(port_name); robot->EnableCommandedMode(); ROS_INFO("Using CAN bus to talk with the robot"); - } + } } messenger.SetupSubscription(); diff --git a/scout_base/src/scout_messenger.cpp b/scout_base/src/scout_messenger.cpp index 47011e0..204d463 100644 --- a/scout_base/src/scout_messenger.cpp +++ b/scout_base/src/scout_messenger.cpp @@ -13,281 +13,267 @@ #include "scout_msgs/ScoutStatus.h" -namespace westonrobot -{ - ScoutROSMessenger::ScoutROSMessenger(ros::NodeHandle *nh) - : scout_(nullptr), nh_(nh) {} +namespace westonrobot { +ScoutROSMessenger::ScoutROSMessenger(ros::NodeHandle *nh) + : scout_(nullptr), nh_(nh) {} - ScoutROSMessenger::ScoutROSMessenger(ScoutBase *scout, ros::NodeHandle *nh) - : scout_(scout), nh_(nh) {} +ScoutROSMessenger::ScoutROSMessenger(std::shared_ptr scout, + ros::NodeHandle *nh) + : scout_(scout), nh_(nh) {} - void ScoutROSMessenger::SetupSubscription() - { - // odometry publisher - odom_publisher_ = nh_->advertise(odom_topic_name_, 50); - status_publisher_ = - nh_->advertise("/scout_status", 10); +void ScoutROSMessenger::SetupSubscription() { + // odometry publisher + odom_publisher_ = nh_->advertise(odom_topic_name_, 50); + status_publisher_ = + nh_->advertise("/scout_status", 10); - // cmd subscriber - motion_cmd_subscriber_ = nh_->subscribe( - "/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this); - light_cmd_subscriber_ = nh_->subscribe( - "/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this); - } + // cmd subscriber + motion_cmd_subscriber_ = nh_->subscribe( + "/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this); + light_cmd_subscriber_ = nh_->subscribe( + "/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this); +} - void ScoutROSMessenger::TwistCmdCallback( - const geometry_msgs::Twist::ConstPtr &msg) - { - if (!simulated_robot_) - { - scout_->SetMotionCommand(msg->linear.x, msg->angular.z); - } - else - { - std::lock_guard guard(twist_mutex_); - current_twist_ = *msg.get(); - } - // ROS_INFO("cmd received:%f, %f", msg->linear.x, msg->angular.z); - } - - void ScoutROSMessenger::GetCurrentMotionCmdForSim(double &linear, - double &angular) - { +void ScoutROSMessenger::TwistCmdCallback( + const geometry_msgs::Twist::ConstPtr &msg) { + if (!simulated_robot_) { + scout_->SetMotionCommand(msg->linear.x, msg->angular.z); + } else { std::lock_guard guard(twist_mutex_); - linear = current_twist_.linear.x; - angular = current_twist_.angular.z; + current_twist_ = *msg.get(); } + // ROS_INFO("cmd received:%f, %f", msg->linear.x, msg->angular.z); +} - void ScoutROSMessenger::LightCmdCallback( - const scout_msgs::ScoutLightCmd::ConstPtr &msg) - { - if (!simulated_robot_) - { - if (msg->cmd_ctrl_allowed) - { - ScoutLightCmd cmd; +void ScoutROSMessenger::GetCurrentMotionCmdForSim(double &linear, + double &angular) { + std::lock_guard guard(twist_mutex_); + linear = current_twist_.linear.x; + angular = current_twist_.angular.z; +} - switch (msg->front_mode) - { - case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: - { - cmd.front_mode = CONST_OFF; - break; - } - case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: - { - cmd.front_mode = CONST_ON; - break; - } - case scout_msgs::ScoutLightCmd::LIGHT_BREATH: - { - cmd.front_mode = BREATH; - break; - } - case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: - { - cmd.front_mode = CUSTOM; - cmd.front_custom_value = msg->front_custom_value; - break; - } - } - // not meant to be controlled by user for now - // switch (msg->rear_mode) - // { - // case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: - // { - // cmd.rear_mode = CONST_OFF; - // break; - // } - // case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: - // { - // cmd.rear_mode = CONST_ON; - // break; - // } - // case scout_msgs::ScoutLightCmd::LIGHT_BREATH: - // { - // cmd.rear_mode = BREATH; - // break; - // } - // case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: - // { - // cmd.rear_mode = CUSTOM; - // cmd.rear_custom_value = msg->rear_custom_value; - // break; - // } - // } - cmd.cmd_ctrl_allowed = true; - scout_->SetLightCommand(cmd); - } - else - { - scout_->DisableLightControl(); - } - } - else - { - std::cout << "simulated robot received light control cmd" << std::endl; - } - } +void ScoutROSMessenger::LightCmdCallback( + const scout_msgs::ScoutLightCmd::ConstPtr &msg) { + if (!simulated_robot_) { + // if (msg->cmd_ctrl_allowed) { + // ScoutLightCmd cmd; - void ScoutROSMessenger::PublishStateToROS() - { - current_time_ = ros::Time::now(); - double dt = (current_time_ - last_time_).toSec(); - - static bool init_run = true; - if (init_run) - { - last_time_ = current_time_; - init_run = false; - return; - } - - auto state = scout_->GetScoutState(); - - // publish scout state message - scout_msgs::ScoutStatus status_msg; - - status_msg.header.stamp = current_time_; - - status_msg.linear_velocity = state.motion_state.linear_velocity; - status_msg.angular_velocity = state.motion_state.angular_velocity; - - 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; - - for (int i = 0; i < 4; ++i) - { - // actuator_hs_state - uint8_t motor_id = state.actuator_hs_state[i].motor_id; - - status_msg.actuator_states[motor_id].rpm = state.actuator_hs_state[i].rpm; - status_msg.actuator_states[motor_id].current = state.actuator_hs_state[i].current; - status_msg.actuator_states[motor_id].pulse_count = state.actuator_hs_state[i].pulse_count; - - // actuator_ls_state - motor_id = state.actuator_ls_state[i].motor_id; - - status_msg.actuator_states[motor_id].driver_voltage = state.actuator_ls_state[i].driver_voltage; - status_msg.actuator_states[motor_id].driver_temperature = state.actuator_ls_state[i].driver_temperature; - status_msg.actuator_states[motor_id].motor_temperature = state.actuator_ls_state[i].motor_temperature; - status_msg.actuator_states[motor_id].driver_state = state.actuator_ls_state[i].driver_state; - } - - status_msg.light_control_enabled = state.light_state.cmd_ctrl_allowed; - 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_publisher_.publish(status_msg); - - // publish odometry and tf - PublishOdometryToROS(state.motion_state.linear_velocity, state.motion_state.angular_velocity, dt); - - // record time for next integration - last_time_ = current_time_; - } - - void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) - { - current_time_ = ros::Time::now(); - - double dt = (current_time_ - last_time_).toSec(); - - static bool init_run = true; - if (init_run) - { - last_time_ = current_time_; - init_run = false; - return; - } - - // publish scout state message - scout_msgs::ScoutStatus status_msg; - - status_msg.header.stamp = current_time_; - - status_msg.linear_velocity = linear; - status_msg.angular_velocity = angular; - - status_msg.vehicle_state = 0x00; - status_msg.control_mode = 0x01; - status_msg.error_code = 0x00; - status_msg.battery_voltage = 29.5; - - // for (int i = 0; i < 4; ++i) - // { - // status_msg.motor_states[i].current = state.motor_states[i].current; - // status_msg.motor_states[i].rpm = state.motor_states[i].rpm; - // status_msg.motor_states[i].temperature = - // state.motor_states[i].temperature; + // switch (msg->front_mode) { + // case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: { + // cmd.front_mode = CONST_OFF; + // break; + // } + // case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: { + // cmd.front_mode = CONST_ON; + // break; + // } + // case scout_msgs::ScoutLightCmd::LIGHT_BREATH: { + // cmd.front_mode = BREATH; + // break; + // } + // case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: { + // cmd.front_mode = CUSTOM; + // cmd.front_custom_value = msg->front_custom_value; + // break; + // } + // } + // // not meant to be controlled by user for now + // // switch (msg->rear_mode) + // // { + // // case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: + // // { + // // cmd.rear_mode = CONST_OFF; + // // break; + // // } + // // case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: + // // { + // // cmd.rear_mode = CONST_ON; + // // break; + // // } + // // case scout_msgs::ScoutLightCmd::LIGHT_BREATH: + // // { + // // cmd.rear_mode = BREATH; + // // break; + // // } + // // case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: + // // { + // // cmd.rear_mode = CUSTOM; + // // cmd.rear_custom_value = msg->rear_custom_value; + // // break; + // // } + // // } + // cmd.cmd_ctrl_allowed = true; + // scout_->SetLightCommand(cmd); + // } else { + // scout_->DisableLightControl(); // } + } else { + std::cout << "simulated robot received light control cmd" << std::endl; + } +} - status_msg.light_control_enabled = false; - // status_msg.front_light_state.mode = state.front_light_state.mode; - // status_msg.front_light_state.custom_value = - // state.front_light_state.custom_value; status_msg.rear_light_state.mode = - // state.rear_light_state.mode; status_msg.rear_light_state.custom_value = - // state.front_light_state.custom_value; +void ScoutROSMessenger::PublishStateToROS() { + current_time_ = ros::Time::now(); + double dt = (current_time_ - last_time_).toSec(); - status_publisher_.publish(status_msg); - - // publish odometry and tf - PublishOdometryToROS(linear, angular, dt); - - // record time for next integration + static bool init_run = true; + if (init_run) { last_time_ = current_time_; + init_run = false; + return; } - void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular, - double dt) - { - // perform numerical integration to get an estimation of pose - linear_speed_ = linear; - angular_speed_ = angular; + auto state = scout_->GetRobotState(); - double d_x = linear_speed_ * std::cos(theta_) * dt; - double d_y = linear_speed_ * std::sin(theta_) * dt; - double d_theta = angular_speed_ * dt; + // publish scout state message + scout_msgs::ScoutStatus status_msg; - position_x_ += d_x; - position_y_ += d_y; - theta_ += d_theta; + status_msg.header.stamp = current_time_; - geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_); + status_msg.linear_velocity = state.motion_state.linear_velocity; + status_msg.angular_velocity = state.motion_state.angular_velocity; - // publish tf transformation - geometry_msgs::TransformStamped tf_msg; - tf_msg.header.stamp = current_time_; - tf_msg.header.frame_id = odom_frame_; - tf_msg.child_frame_id = base_frame_; + 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; - 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; + auto actuator = scout_->GetActuatorState(); - tf_broadcaster_.sendTransform(tf_msg); + for (int i = 0; i < 4; ++i) { + // actuator_hs_state + uint8_t motor_id = actuator.actuator_hs_state[i].motor_id; - // publish odometry and tf messages - nav_msgs::Odometry odom_msg; - odom_msg.header.stamp = current_time_; - odom_msg.header.frame_id = odom_frame_; - odom_msg.child_frame_id = base_frame_; + 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; - 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; + // actuator_ls_state + motor_id = actuator.actuator_ls_state[i].motor_id; - odom_msg.twist.twist.linear.x = linear_speed_; - odom_msg.twist.twist.linear.y = 0.0; - odom_msg.twist.twist.angular.z = angular_speed_; - - odom_publisher_.publish(odom_msg); + 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; } -} // namespace westonrobot \ No newline at end of file + + 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_publisher_.publish(status_msg); + + // publish odometry and tf + PublishOdometryToROS(state.motion_state.linear_velocity, + state.motion_state.angular_velocity, dt); + + // record time for next integration + last_time_ = current_time_; +} + +void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) { + current_time_ = ros::Time::now(); + + double dt = (current_time_ - last_time_).toSec(); + + static bool init_run = true; + if (init_run) { + last_time_ = current_time_; + init_run = false; + return; + } + + // publish scout state message + scout_msgs::ScoutStatus status_msg; + + status_msg.header.stamp = current_time_; + + status_msg.linear_velocity = linear; + status_msg.angular_velocity = angular; + + status_msg.vehicle_state = 0x00; + status_msg.control_mode = 0x01; + status_msg.error_code = 0x00; + status_msg.battery_voltage = 29.5; + + // for (int i = 0; i < 4; ++i) + // { + // status_msg.motor_states[i].current = state.motor_states[i].current; + // status_msg.motor_states[i].rpm = state.motor_states[i].rpm; + // status_msg.motor_states[i].temperature = + // state.motor_states[i].temperature; + // } + + status_msg.light_control_enabled = false; + // status_msg.front_light_state.mode = state.front_light_state.mode; + // status_msg.front_light_state.custom_value = + // state.front_light_state.custom_value; status_msg.rear_light_state.mode = + // state.rear_light_state.mode; status_msg.rear_light_state.custom_value = + // state.front_light_state.custom_value; + + status_publisher_.publish(status_msg); + + // publish odometry and tf + PublishOdometryToROS(linear, angular, dt); + + // record time for next integration + last_time_ = current_time_; +} + +void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular, + double dt) { + // perform numerical integration to get an estimation of pose + linear_speed_ = linear; + angular_speed_ = angular; + + double d_x = linear_speed_ * std::cos(theta_) * dt; + double d_y = linear_speed_ * std::sin(theta_) * dt; + double d_theta = angular_speed_ * dt; + + position_x_ += d_x; + position_y_ += d_y; + theta_ += d_theta; + + geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_); + + // publish tf transformation + geometry_msgs::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_broadcaster_.sendTransform(tf_msg); + + // publish odometry and tf messages + nav_msgs::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.twist.twist.linear.x = linear_speed_; + odom_msg.twist.twist.linear.y = 0.0; + odom_msg.twist.twist.angular.z = angular_speed_; + + odom_publisher_.publish(odom_msg); +} +} // namespace westonrobot \ No newline at end of file