mirrors/scout_ros
1
0
Fork 0
mirror of https://github.com/westonrobot/scout_ros.git synced 2026-01-12 03:15:01 +08:00
Code Issues Packages Projects Releases Wiki Activity

updated to use next branch of ugv_sdk

Browse Source
This commit is contained in:
Ruixiang Du
2021-10-05 15:04:21 +08:00
parent fd7ae717cb
commit 117f8def26
4 changed files with 308 additions and 322 deletions
Download Patch File Download Diff File Expand all files Collapse all files

85
scout_base/include/scout_base/scout_messenger.hpp
View File

@@ -1,9 +1,9 @@
/* /*
* scout_messenger.hpp * scout_messenger.hpp
* *
* Created on: Jun 14, 2019 10:24 * Created on: Jun 14, 2019 10:24
* Description: * Description:
* *
* Copyright (c) 2019 Ruixiang Du (rdu) * Copyright (c) 2019 Ruixiang Du (rdu)
*/ */
@@ -11,6 +11,7 @@
#define SCOUT_MESSENGER_HPP #define SCOUT_MESSENGER_HPP
#include <string> #include <string>
#include <mutex>
#include <ros/ros.h> #include <ros/ros.h>
#include <nav_msgs/Odometry.h> #include <nav_msgs/Odometry.h>
@@ -18,57 +19,55 @@
#include <tf2_ros/transform_broadcaster.h> #include <tf2_ros/transform_broadcaster.h>
#include "scout_msgs/ScoutLightCmd.h" #include "scout_msgs/ScoutLightCmd.h"
#include "ugv_sdk/scout_base.hpp" #include "ugv_sdk/mobile_robot/scout_robot.hpp"
namespace westonrobot namespace westonrobot {
{ class ScoutROSMessenger {
class ScoutROSMessenger public:
{ explicit ScoutROSMessenger(ros::NodeHandle *nh);
public: ScoutROSMessenger(std::shared_ptr<ScoutRobot> scout, ros::NodeHandle *nh);
explicit ScoutROSMessenger(ros::NodeHandle *nh);
ScoutROSMessenger(ScoutBase *scout, ros::NodeHandle *nh);
std::string odom_frame_; std::string odom_frame_;
std::string base_frame_; std::string base_frame_;
std::string odom_topic_name_; std::string odom_topic_name_;
bool simulated_robot_ = false; bool simulated_robot_ = false;
int sim_control_rate_ = 50; int sim_control_rate_ = 50;
void SetupSubscription(); void SetupSubscription();
void PublishStateToROS(); void PublishStateToROS();
void PublishSimStateToROS(double linear, double angular); void PublishSimStateToROS(double linear, double angular);
void GetCurrentMotionCmdForSim(double &linear, double &angular); void GetCurrentMotionCmdForSim(double &linear, double &angular);
private: private:
ScoutBase *scout_; std::shared_ptr<ScoutRobot> scout_;
ros::NodeHandle *nh_; ros::NodeHandle *nh_;
std::mutex twist_mutex_; std::mutex twist_mutex_;
geometry_msgs::Twist current_twist_; geometry_msgs::Twist current_twist_;
ros::Publisher odom_publisher_; ros::Publisher odom_publisher_;
ros::Publisher status_publisher_; ros::Publisher status_publisher_;
ros::Subscriber motion_cmd_subscriber_; ros::Subscriber motion_cmd_subscriber_;
ros::Subscriber light_cmd_subscriber_; ros::Subscriber light_cmd_subscriber_;
tf2_ros::TransformBroadcaster tf_broadcaster_; tf2_ros::TransformBroadcaster tf_broadcaster_;
// speed variables // speed variables
double linear_speed_ = 0.0; double linear_speed_ = 0.0;
double angular_speed_ = 0.0; double angular_speed_ = 0.0;
double position_x_ = 0.0; double position_x_ = 0.0;
double position_y_ = 0.0; double position_y_ = 0.0;
double theta_ = 0.0; double theta_ = 0.0;
ros::Time last_time_; ros::Time last_time_;
ros::Time current_time_; ros::Time current_time_;
void TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg); void TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg);
void LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg); void LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg);
void PublishOdometryToROS(double linear, double angular, double dt); void PublishOdometryToROS(double linear, double angular, double dt);
}; };
} // namespace westonrobot } // namespace westonrobot
#endif /* SCOUT_MESSENGER_HPP */ #endif /* SCOUT_MESSENGER_HPP */

42
scout_base/include/scout_base/scout_params.hpp
View File

@@ -1,33 +1,33 @@
/* /*
* scout_params.hpp * scout_params.hpp
* *
* Created on: Sep 27, 2019 15:08 * Created on: Sep 27, 2019 15:08
* Description: * Description:
* *
* Copyright (c) 2020 Ruixiang Du (rdu) * Copyright (c) 2020 Ruixiang Du (rdu)
*/ */
#ifndef SCOUT_PARAMS_HPP #ifndef SCOUT_PARAMS_HPP
#define SCOUT_PARAMS_HPP #define SCOUT_PARAMS_HPP
namespace westonrobot namespace westonrobot {
{ struct ScoutParams {
struct ScoutParams /* Scout Parameters */
{ static constexpr double max_steer_angle = 30.0; // in degree
/* 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 track =
static constexpr double wheelbase = 0.498; // in meter (front & rear wheel distance) 0.58306; // in meter (left & right wheel distance)
static constexpr double wheel_radius = 0.165; // in meter 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 // from user manual v1.2.8 P18
// max linear velocity: 1.5 m/s // max linear velocity: 1.5 m/s
// max angular velocity: 0.7853 rad/s // max angular velocity: 0.7853 rad/s
static constexpr double max_linear_speed = 1.5; // in m/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_angular_speed = 0.7853; // in rad/s
static constexpr double max_speed_cmd = 10.0; // in rad/s static constexpr double max_speed_cmd = 10.0; // in rad/s
}; };
} // namespace westonrobot } // namespace westonrobot
#endif /* SCOUT_PARAMS_HPP */ #endif /* SCOUT_PARAMS_HPP */

11
scout_base/src/scout_base_node.cpp
View File

@@ -1,16 +1,17 @@
#include <memory> #include <memory>
#include <csignal>
#include <ros/ros.h> #include <ros/ros.h>
#include <nav_msgs/Odometry.h> #include <nav_msgs/Odometry.h>
#include <sensor_msgs/JointState.h> #include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h> #include <tf/transform_broadcaster.h>
#include "ugv_sdk/scout_base.hpp" #include "ugv_sdk/mobile_robot/scout_robot.hpp"
#include "scout_base/scout_messenger.hpp" #include "scout_base/scout_messenger.hpp"
using namespace westonrobot; using namespace westonrobot;
std::shared_ptr<ScoutBase> robot; std::shared_ptr<ScoutRobot> robot;
bool keep_run = true; bool keep_run = true;
void DetachRobot(int signal) { keep_run = false; } void DetachRobot(int signal) { keep_run = false; }
@@ -23,8 +24,8 @@ int main(int argc, char **argv) {
std::signal(SIGINT, DetachRobot); std::signal(SIGINT, DetachRobot);
// instantiate a robot object // instantiate a robot object
robot = std::make_shared<ScoutBase>(); robot = std::make_shared<ScoutRobot>();
ScoutROSMessenger messenger(robot.get(), &node); ScoutROSMessenger messenger(robot, &node);
// fetch parameters before connecting to robot // fetch parameters before connecting to robot
std::string port_name; std::string port_name;
@@ -45,7 +46,7 @@ int main(int argc, char **argv) {
robot->Connect(port_name); robot->Connect(port_name);
robot->EnableCommandedMode(); robot->EnableCommandedMode();
ROS_INFO("Using CAN bus to talk with the robot"); ROS_INFO("Using CAN bus to talk with the robot");
} }
} }
messenger.SetupSubscription(); messenger.SetupSubscription();

492
scout_base/src/scout_messenger.cpp
View File

@@ -13,281 +13,267 @@
#include "scout_msgs/ScoutStatus.h" #include "scout_msgs/ScoutStatus.h"
namespace westonrobot namespace westonrobot {
{ ScoutROSMessenger::ScoutROSMessenger(ros::NodeHandle *nh)
ScoutROSMessenger::ScoutROSMessenger(ros::NodeHandle *nh) : scout_(nullptr), nh_(nh) {}
: scout_(nullptr), nh_(nh) {}
ScoutROSMessenger::ScoutROSMessenger(ScoutBase *scout, ros::NodeHandle *nh) ScoutROSMessenger::ScoutROSMessenger(std::shared_ptr<ScoutRobot> scout,
: scout_(scout), nh_(nh) {} ros::NodeHandle *nh)
: scout_(scout), nh_(nh) {}
void ScoutROSMessenger::SetupSubscription() void ScoutROSMessenger::SetupSubscription() {
{ // odometry publisher
// odometry publisher odom_publisher_ = nh_->advertise<nav_msgs::Odometry>(odom_topic_name_, 50);
odom_publisher_ = nh_->advertise<nav_msgs::Odometry>(odom_topic_name_, 50); status_publisher_ =
status_publisher_ = nh_->advertise<scout_msgs::ScoutStatus>("/scout_status", 10);
nh_->advertise<scout_msgs::ScoutStatus>("/scout_status", 10);
// cmd subscriber // cmd subscriber
motion_cmd_subscriber_ = nh_->subscribe<geometry_msgs::Twist>( motion_cmd_subscriber_ = nh_->subscribe<geometry_msgs::Twist>(
"/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this); "/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this);
light_cmd_subscriber_ = nh_->subscribe<scout_msgs::ScoutLightCmd>( light_cmd_subscriber_ = nh_->subscribe<scout_msgs::ScoutLightCmd>(
"/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this); "/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this);
} }
void ScoutROSMessenger::TwistCmdCallback( void ScoutROSMessenger::TwistCmdCallback(
const geometry_msgs::Twist::ConstPtr &msg) const geometry_msgs::Twist::ConstPtr &msg) {
{ if (!simulated_robot_) {
if (!simulated_robot_) scout_->SetMotionCommand(msg->linear.x, msg->angular.z);
{ } else {
scout_->SetMotionCommand(msg->linear.x, msg->angular.z);
}
else
{
std::lock_guard<std::mutex> 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)
{
std::lock_guard<std::mutex> guard(twist_mutex_); std::lock_guard<std::mutex> guard(twist_mutex_);
linear = current_twist_.linear.x; current_twist_ = *msg.get();
angular = current_twist_.angular.z;
} }
// ROS_INFO("cmd received:%f, %f", msg->linear.x, msg->angular.z);
}
void ScoutROSMessenger::LightCmdCallback( void ScoutROSMessenger::GetCurrentMotionCmdForSim(double &linear,
const scout_msgs::ScoutLightCmd::ConstPtr &msg) double &angular) {
{ std::lock_guard<std::mutex> guard(twist_mutex_);
if (!simulated_robot_) linear = current_twist_.linear.x;
{ angular = current_twist_.angular.z;
if (msg->cmd_ctrl_allowed) }
{
ScoutLightCmd cmd;
switch (msg->front_mode) void ScoutROSMessenger::LightCmdCallback(
{ const scout_msgs::ScoutLightCmd::ConstPtr &msg) {
case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: if (!simulated_robot_) {
{ // if (msg->cmd_ctrl_allowed) {
cmd.front_mode = CONST_OFF; // ScoutLightCmd cmd;
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::PublishStateToROS() // switch (msg->front_mode) {
{ // case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: {
current_time_ = ros::Time::now(); // cmd.front_mode = CONST_OFF;
double dt = (current_time_ - last_time_).toSec(); // break;
// }
static bool init_run = true; // case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: {
if (init_run) // cmd.front_mode = CONST_ON;
{ // break;
last_time_ = current_time_; // }
init_run = false; // case scout_msgs::ScoutLightCmd::LIGHT_BREATH: {
return; // cmd.front_mode = BREATH;
} // break;
// }
auto state = scout_->GetScoutState(); // case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: {
// cmd.front_mode = CUSTOM;
// publish scout state message // cmd.front_custom_value = msg->front_custom_value;
scout_msgs::ScoutStatus status_msg; // break;
// }
status_msg.header.stamp = current_time_; // }
// // not meant to be controlled by user for now
status_msg.linear_velocity = state.motion_state.linear_velocity; // // switch (msg->rear_mode)
status_msg.angular_velocity = state.motion_state.angular_velocity; // // {
// // case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
status_msg.vehicle_state = state.system_state.vehicle_state; // // {
status_msg.control_mode = state.system_state.control_mode; // // cmd.rear_mode = CONST_OFF;
status_msg.error_code = state.system_state.error_code; // // break;
status_msg.battery_voltage = state.system_state.battery_voltage; // // }
// // case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
for (int i = 0; i < 4; ++i) // // {
{ // // cmd.rear_mode = CONST_ON;
// actuator_hs_state // // break;
uint8_t motor_id = state.actuator_hs_state[i].motor_id; // // }
// // case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
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; // // cmd.rear_mode = BREATH;
status_msg.actuator_states[motor_id].pulse_count = state.actuator_hs_state[i].pulse_count; // // break;
// // }
// actuator_ls_state // // case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
motor_id = state.actuator_ls_state[i].motor_id; // // {
// // cmd.rear_mode = CUSTOM;
status_msg.actuator_states[motor_id].driver_voltage = state.actuator_ls_state[i].driver_voltage; // // cmd.rear_custom_value = msg->rear_custom_value;
status_msg.actuator_states[motor_id].driver_temperature = state.actuator_ls_state[i].driver_temperature; // // break;
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; // // }
} // cmd.cmd_ctrl_allowed = true;
// scout_->SetLightCommand(cmd);
status_msg.light_control_enabled = state.light_state.cmd_ctrl_allowed; // } else {
status_msg.front_light_state.mode = state.light_state.front_light.mode; // scout_->DisableLightControl();
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;
// } // }
} else {
std::cout << "simulated robot received light control cmd" << std::endl;
}
}
status_msg.light_control_enabled = false; void ScoutROSMessenger::PublishStateToROS() {
// status_msg.front_light_state.mode = state.front_light_state.mode; current_time_ = ros::Time::now();
// status_msg.front_light_state.custom_value = double dt = (current_time_ - last_time_).toSec();
// 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); static bool init_run = true;
if (init_run) {
// publish odometry and tf
PublishOdometryToROS(linear, angular, dt);
// record time for next integration
last_time_ = current_time_; last_time_ = current_time_;
init_run = false;
return;
} }
void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular, auto state = scout_->GetRobotState();
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; // publish scout state message
double d_y = linear_speed_ * std::sin(theta_) * dt; scout_msgs::ScoutStatus status_msg;
double d_theta = angular_speed_ * dt;
position_x_ += d_x; status_msg.header.stamp = current_time_;
position_y_ += d_y;
theta_ += d_theta;
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 status_msg.vehicle_state = state.system_state.vehicle_state;
geometry_msgs::TransformStamped tf_msg; status_msg.control_mode = state.system_state.control_mode;
tf_msg.header.stamp = current_time_; status_msg.error_code = state.system_state.error_code;
tf_msg.header.frame_id = odom_frame_; status_msg.battery_voltage = state.system_state.battery_voltage;
tf_msg.child_frame_id = base_frame_;
tf_msg.transform.translation.x = position_x_; auto actuator = scout_->GetActuatorState();
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); 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 status_msg.actuator_states[motor_id].rpm =
nav_msgs::Odometry odom_msg; actuator.actuator_hs_state[i].rpm;
odom_msg.header.stamp = current_time_; status_msg.actuator_states[motor_id].current =
odom_msg.header.frame_id = odom_frame_; actuator.actuator_hs_state[i].current;
odom_msg.child_frame_id = base_frame_; status_msg.actuator_states[motor_id].pulse_count =
actuator.actuator_hs_state[i].pulse_count;
odom_msg.pose.pose.position.x = position_x_; // actuator_ls_state
odom_msg.pose.pose.position.y = position_y_; motor_id = actuator.actuator_ls_state[i].motor_id;
odom_msg.pose.pose.position.z = 0.0;
odom_msg.pose.pose.orientation = odom_quat;
odom_msg.twist.twist.linear.x = linear_speed_; status_msg.actuator_states[motor_id].driver_voltage =
odom_msg.twist.twist.linear.y = 0.0; actuator.actuator_ls_state[i].driver_voltage;
odom_msg.twist.twist.angular.z = angular_speed_; status_msg.actuator_states[motor_id].driver_temperature =
actuator.actuator_ls_state[i].driver_temp;
odom_publisher_.publish(odom_msg); 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
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