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

updated scout_base to work with scout using both v1 and v2 protocol

Browse Source
This commit is contained in:
Ruixiang Du
2021-10-05 17:33:40 +08:00
parent 7d9f027137
commit 27f4e5d411
5 changed files with 281 additions and 349 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
README.md
View File

@@ -1,10 +1,5 @@
# ROS Packages for Scout Mobile Robot # ROS Packages for Scout Mobile Robot
**Important Note:** Currently we're transitioning the communication protocol from version 1 to version 2. Please check with Weston Robot or AgileX Robotics to confirm which version your robot is using.
* V1 Protocol: master branch of scout_ros and ugv_sdk
* V2 Protocol: v2.x branch of scout_ros and ugv_sdk
## Packages ## Packages
This repository contains minimal packages to control the scout robot using ROS. This repository contains minimal packages to control the scout robot using ROS.
@@ -54,14 +49,13 @@ Nvidia Jeston TX2/Xavier/XavierNX have CAN controller(s) integrated in the main
``` ```
$ cd ~/catkin_ws/src $ cd ~/catkin_ws/src
$ git clone https://github.com/westonrobot/async_port.git $ git clone https://github.com/westonrobot/ugv_sdk.git
$ git clone -b v2.x https://github.com/westonrobot/ugv_sdk.git $ git clone https://github.com/westonrobot/scout_ros.git
$ git clone -b v2.x https://github.com/westonrobot/scout_ros.git
$ cd .. $ cd ..
$ catkin_make $ catkin_make
``` ```
4. Launch ROS nodes 3. Launch ROS nodes
* Start the base node for the real robot * Start the base node for the real robot

9
scout_base/CMakeLists.txt
View File

@@ -37,15 +37,10 @@ catkin_package(
## Your package locations should be listed before other locations ## Your package locations should be listed before other locations
include_directories( include_directories(
include include
${catkin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
)
add_library(scout_messenger STATIC src/scout_messenger.cpp)
target_link_libraries(scout_messenger ${catkin_LIBRARIES})
add_dependencies(scout_messenger ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_executable(scout_base_node src/scout_base_node.cpp) add_executable(scout_base_node src/scout_base_node.cpp)
target_link_libraries(scout_base_node scout_messenger ${catkin_LIBRARIES}) target_link_libraries(scout_base_node ${catkin_LIBRARIES})
############# #############
## Install ## ## Install ##

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

@@ -18,25 +18,40 @@
// #include <tf/transform_broadcaster.h> // #include <tf/transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h> #include <tf2_ros/transform_broadcaster.h>
#include "scout_msgs/ScoutStatus.h"
#include "scout_msgs/ScoutLightCmd.h" #include "scout_msgs/ScoutLightCmd.h"
#include "ugv_sdk/mobile_robot/scout_robot.hpp" #include "ugv_sdk/mobile_robot/scout_robot.hpp"
namespace westonrobot { namespace westonrobot {
template <typename ScoutType>
class ScoutMessenger { class ScoutMessenger {
public: public:
explicit ScoutMessenger(ros::NodeHandle *nh); ScoutMessenger(std::shared_ptr<ScoutType> scout, ros::NodeHandle *nh)
ScoutMessenger(std::shared_ptr<ScoutRobot> scout, ros::NodeHandle *nh); : scout_(scout), nh_(nh) {}
void SetOdometryFrame(std::string frame); void SetOdometryFrame(std::string frame) { odom_frame_ = frame; }
void SetBaseFrame(std::string frame); void SetBaseFrame(std::string frame) { base_frame_ = frame; }
void SetOdometryTopicName(std::string name); void SetOdometryTopicName(std::string name) { odom_topic_name_ = name; }
void SetSimulationMode(int loop_rate);
void Run(); void SetSimulationMode(int loop_rate) {
void Stop(); simulated_robot_ = true;
sim_control_rate_ = loop_rate;
}
void Run() {
SetupSubscription();
// publish robot state at 50Hz while listening to twist commands
ros::Rate rate(50);
while (ros::ok()) {
PublishStateToROS();
ros::spinOnce();
rate.sleep();
}
}
private: private:
std::shared_ptr<ScoutRobot> scout_; std::shared_ptr<ScoutType> scout_;
ros::NodeHandle *nh_; ros::NodeHandle *nh_;
std::string odom_frame_; std::string odom_frame_;
@@ -46,7 +61,6 @@ class ScoutMessenger {
bool simulated_robot_ = false; bool simulated_robot_ = false;
int sim_control_rate_ = 50; int sim_control_rate_ = 50;
std::atomic<bool> keep_running_;
std::mutex twist_mutex_; std::mutex twist_mutex_;
geometry_msgs::Twist current_twist_; geometry_msgs::Twist current_twist_;
@@ -67,15 +81,254 @@ class ScoutMessenger {
ros::Time last_time_; ros::Time last_time_;
ros::Time current_time_; ros::Time current_time_;
void SetupSubscription(); void SetupSubscription() {
// odometry publisher
odom_pub_ = nh_->advertise<nav_msgs::Odometry>(odom_topic_name_, 50);
status_pub_ = nh_->advertise<scout_msgs::ScoutStatus>("/scout_status", 10);
void TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg); // cmd subscriber
void LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg); motion_cmd_sub_ = nh_->subscribe<geometry_msgs::Twist>(
void PublishOdometryToROS(double linear, double angular, double dt); "/cmd_vel", 5, &ScoutMessenger::TwistCmdCallback, this);
light_cmd_sub_ = nh_->subscribe<scout_msgs::ScoutLightCmd>(
"/scout_light_control", 5, &ScoutMessenger::LightCmdCallback, this);
}
void PublishStateToROS(); void TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg) {
void PublishSimStateToROS(double linear, double angular); if (!simulated_robot_) {
void GetCurrentMotionCmdForSim(double &linear, double &angular); 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 LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg) {
if (!simulated_robot_) {
// if (msg->cmd_ctrl_allowed) {
// ScoutLightCmd cmd;
// 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 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_pub_.publish(odom_msg);
}
void 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_->GetRobotState();
// 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;
auto actuator = scout_->GetActuatorState();
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].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;
// actuator_ls_state
motor_id = actuator.actuator_ls_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.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.linear_velocity,
state.motion_state.angular_velocity, dt);
// record time for next integration
last_time_ = current_time_;
}
void 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_pub_.publish(status_msg);
// publish odometry and tf
PublishOdometryToROS(linear, angular, dt);
// record time for next integration
last_time_ = current_time_;
}
// void PublishSimStateToROS(double linear, double angular);
// void GetCurrentMotionCmdForSim(double &linear, double &angular);
}; };
} // namespace westonrobot } // namespace westonrobot

14
scout_base/src/scout_base_node.cpp
View File

@@ -13,18 +13,12 @@
using namespace westonrobot; using namespace westonrobot;
std::shared_ptr<ScoutRobot> robot;
std::unique_ptr<ScoutMessenger> messenger;
bool keep_run = true;
void DetachRobot(int signal) { messenger->Stop(); }
int main(int argc, char **argv) { int main(int argc, char **argv) {
// setup ROS node // setup ROS node
ros::init(argc, argv, "scout_node"); ros::init(argc, argv, "scout_node");
ros::NodeHandle node(""), private_node("~"); ros::NodeHandle node(""), private_node("~");
std::signal(SIGINT, DetachRobot); // std::signal(SIGINT, DetachRobot);
// fetch parameters before connecting to robot // fetch parameters before connecting to robot
std::string port_name; std::string port_name;
@@ -52,6 +46,9 @@ int main(int argc, char **argv) {
ROS_INFO("Robot base: Scout"); ROS_INFO("Robot base: Scout");
} }
std::shared_ptr<ScoutRobot> robot;
std::unique_ptr<ScoutMessenger<ScoutRobot>> messenger;
// instantiate a robot object // instantiate a robot object
ProtocolDectctor detector; ProtocolDectctor detector;
if (detector.Connect(port_name)) { if (detector.Connect(port_name)) {
@@ -73,7 +70,8 @@ int main(int argc, char **argv) {
} }
// instantiate a ROS messenger // instantiate a ROS messenger
messenger = std::unique_ptr<ScoutMessenger>(new ScoutMessenger(robot, &node)); messenger = std::unique_ptr<ScoutMessenger<ScoutRobot>>(
new ScoutMessenger<ScoutRobot>(robot, &node));
messenger->SetOdometryFrame(odom_frame); messenger->SetOdometryFrame(odom_frame);
messenger->SetBaseFrame(base_frame); messenger->SetBaseFrame(base_frame);

308
scout_base/src/scout_messenger.cpp
View File

@@ -1,308 +0,0 @@
/*
* scout_messenger.cpp
*
* Created on: Apr 26, 2019 22:14
* Description:
*
* Copyright (c) 2019 Ruixiang Du (rdu)
*/
#include "scout_base/scout_messenger.hpp"
#include <tf/transform_broadcaster.h>
#include "scout_msgs/ScoutStatus.h"
namespace westonrobot {
ScoutMessenger::ScoutMessenger(ros::NodeHandle *nh)
: scout_(nullptr), nh_(nh) {}
ScoutMessenger::ScoutMessenger(std::shared_ptr<ScoutRobot> scout,
ros::NodeHandle *nh)
: scout_(scout), nh_(nh) {}
void ScoutMessenger::SetOdometryFrame(std::string frame) {
odom_frame_ = frame;
}
void ScoutMessenger::SetBaseFrame(std::string frame) { base_frame_ = frame; }
void ScoutMessenger::SetOdometryTopicName(std::string name) {
odom_topic_name_ = name;
}
void ScoutMessenger::SetSimulationMode(int loop_rate) {
simulated_robot_ = true;
sim_control_rate_ = loop_rate;
}
void ScoutMessenger::SetupSubscription() {
// odometry publisher
odom_pub_ = nh_->advertise<nav_msgs::Odometry>(odom_topic_name_, 50);
status_pub_ = nh_->advertise<scout_msgs::ScoutStatus>("/scout_status", 10);
// cmd subscriber
motion_cmd_sub_ = nh_->subscribe<geometry_msgs::Twist>(
"/cmd_vel", 5, &ScoutMessenger::TwistCmdCallback, this);
light_cmd_sub_ = nh_->subscribe<scout_msgs::ScoutLightCmd>(
"/scout_light_control", 5, &ScoutMessenger::LightCmdCallback, this);
}
void ScoutMessenger::Run() {
SetupSubscription();
// publish robot state at 50Hz while listening to twist commands
keep_running_ = true;
ros::Rate rate(50);
while (keep_running_) {
PublishStateToROS();
ros::spinOnce();
rate.sleep();
}
}
void ScoutMessenger::Stop() { keep_running_ = false; }
void ScoutMessenger::TwistCmdCallback(
const geometry_msgs::Twist::ConstPtr &msg) {
if (!simulated_robot_) {
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 ScoutMessenger::GetCurrentMotionCmdForSim(double &linear,
double &angular) {
std::lock_guard<std::mutex> guard(twist_mutex_);
linear = current_twist_.linear.x;
angular = current_twist_.angular.z;
}
void ScoutMessenger::LightCmdCallback(
const scout_msgs::ScoutLightCmd::ConstPtr &msg) {
if (!simulated_robot_) {
// if (msg->cmd_ctrl_allowed) {
// ScoutLightCmd cmd;
// 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 ScoutMessenger::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_->GetRobotState();
// 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;
auto actuator = scout_->GetActuatorState();
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].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;
// actuator_ls_state
motor_id = actuator.actuator_ls_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.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.linear_velocity,
state.motion_state.angular_velocity, dt);
// record time for next integration
last_time_ = current_time_;
}
void ScoutMessenger::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_pub_.publish(status_msg);
// publish odometry and tf
PublishOdometryToROS(linear, angular, dt);
// record time for next integration
last_time_ = current_time_;
}
void ScoutMessenger::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_pub_.publish(odom_msg);
}
} // namespace westonrobot