mirror of
https://github.com/westonrobot/scout_ros.git
synced 2026-01-12 03:15:01 +08:00
updated scout_base to work with scout using both v1 and v2 protocol
This commit is contained in:
Ruixiang Du
12
README.md
12
README.md
@@ -1,10 +1,5 @@
|
||||
# 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
|
||||
|
||||
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
|
||||
$ git clone https://github.com/westonrobot/async_port.git
|
||||
$ git clone -b v2.x https://github.com/westonrobot/ugv_sdk.git
|
||||
$ git clone -b v2.x https://github.com/westonrobot/scout_ros.git
|
||||
$ git clone https://github.com/westonrobot/ugv_sdk.git
|
||||
$ git clone https://github.com/westonrobot/scout_ros.git
|
||||
$ cd ..
|
||||
$ catkin_make
|
||||
```
|
||||
|
||||
4. Launch ROS nodes
|
||||
3. Launch ROS nodes
|
||||
|
||||
* Start the base node for the real robot
|
||||
|
||||
|
||||
@@ -37,15 +37,10 @@ catkin_package(
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(
|
||||
include
|
||||
${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})
|
||||
${catkin_INCLUDE_DIRS})
|
||||
|
||||
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 ##
|
||||
|
||||
@@ -18,25 +18,40 @@
|
||||
// #include <tf/transform_broadcaster.h>
|
||||
#include <tf2_ros/transform_broadcaster.h>
|
||||
|
||||
#include "scout_msgs/ScoutStatus.h"
|
||||
#include "scout_msgs/ScoutLightCmd.h"
|
||||
#include "ugv_sdk/mobile_robot/scout_robot.hpp"
|
||||
|
||||
namespace westonrobot {
|
||||
template <typename ScoutType>
|
||||
class ScoutMessenger {
|
||||
public:
|
||||
explicit ScoutMessenger(ros::NodeHandle *nh);
|
||||
ScoutMessenger(std::shared_ptr<ScoutRobot> scout, ros::NodeHandle *nh);
|
||||
ScoutMessenger(std::shared_ptr<ScoutType> scout, ros::NodeHandle *nh)
|
||||
: scout_(scout), nh_(nh) {}
|
||||
|
||||
void SetOdometryFrame(std::string frame);
|
||||
void SetBaseFrame(std::string frame);
|
||||
void SetOdometryTopicName(std::string name);
|
||||
void SetSimulationMode(int loop_rate);
|
||||
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 Run();
|
||||
void Stop();
|
||||
void SetSimulationMode(int loop_rate) {
|
||||
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:
|
||||
std::shared_ptr<ScoutRobot> scout_;
|
||||
std::shared_ptr<ScoutType> scout_;
|
||||
ros::NodeHandle *nh_;
|
||||
|
||||
std::string odom_frame_;
|
||||
@@ -46,7 +61,6 @@ class ScoutMessenger {
|
||||
bool simulated_robot_ = false;
|
||||
int sim_control_rate_ = 50;
|
||||
|
||||
std::atomic<bool> keep_running_;
|
||||
std::mutex twist_mutex_;
|
||||
geometry_msgs::Twist current_twist_;
|
||||
|
||||
@@ -67,15 +81,254 @@ class ScoutMessenger {
|
||||
ros::Time last_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);
|
||||
void LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg);
|
||||
void PublishOdometryToROS(double linear, double angular, double dt);
|
||||
// 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 PublishStateToROS();
|
||||
void PublishSimStateToROS(double linear, double angular);
|
||||
void GetCurrentMotionCmdForSim(double &linear, double &angular);
|
||||
void 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 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
|
||||
|
||||
|
||||
@@ -13,18 +13,12 @@
|
||||
|
||||
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) {
|
||||
// setup ROS node
|
||||
ros::init(argc, argv, "scout_node");
|
||||
ros::NodeHandle node(""), private_node("~");
|
||||
|
||||
std::signal(SIGINT, DetachRobot);
|
||||
// std::signal(SIGINT, DetachRobot);
|
||||
|
||||
// fetch parameters before connecting to robot
|
||||
std::string port_name;
|
||||
@@ -52,6 +46,9 @@ int main(int argc, char **argv) {
|
||||
ROS_INFO("Robot base: Scout");
|
||||
}
|
||||
|
||||
std::shared_ptr<ScoutRobot> robot;
|
||||
std::unique_ptr<ScoutMessenger<ScoutRobot>> messenger;
|
||||
|
||||
// instantiate a robot object
|
||||
ProtocolDectctor detector;
|
||||
if (detector.Connect(port_name)) {
|
||||
@@ -73,7 +70,8 @@ int main(int argc, char **argv) {
|
||||
}
|
||||
|
||||
// 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->SetBaseFrame(base_frame);
|
||||
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user