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

saved work

Browse Source
This commit is contained in:
Ruixiang Du
2020-08-28 18:55:56 +08:00
parent 2d70612bc0
commit f5370e2f3e
1 changed files with 210 additions and 231 deletions
Download Patch File Download Diff File Expand all files Collapse all files

123
scout_base/src/scout_messenger.cpp
View File

@@ -13,100 +13,84 @@
#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) : scout_(scout), nh_(nh) ScoutROSMessenger::ScoutROSMessenger(ScoutBase *scout, 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_frame_, 50); odom_publisher_ = nh_->advertise<nav_msgs::Odometry>(odom_frame_, 50);
status_publisher_ = nh_->advertise<scout_msgs::ScoutStatus>("/scout_status", 10); status_publisher_ =
nh_->advertise<scout_msgs::ScoutStatus>("/scout_status", 10);
// cmd subscriber // cmd subscriber
motion_cmd_subscriber_ = nh_->subscribe<geometry_msgs::Twist>("/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this); //不启用平滑包则订阅“cmd_vel” motion_cmd_subscriber_ = nh_->subscribe<geometry_msgs::Twist>(
light_cmd_subscriber_ = nh_->subscribe<scout_msgs::ScoutLightCmd>("/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this); "/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this);
light_cmd_subscriber_ = nh_->subscribe<scout_msgs::ScoutLightCmd>(
"/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this);
} }
void ScoutROSMessenger::TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg) void ScoutROSMessenger::TwistCmdCallback(
{ const geometry_msgs::Twist::ConstPtr &msg) {
if (!simulated_robot_) if (!simulated_robot_) {
{
scout_->SetMotionCommand(msg->linear.x, msg->angular.z); scout_->SetMotionCommand(msg->linear.x, msg->angular.z);
} } else {
else
{
std::lock_guard<std::mutex> guard(twist_mutex_); std::lock_guard<std::mutex> guard(twist_mutex_);
current_twist_ = *msg.get(); current_twist_ = *msg.get();
} }
// ROS_INFO("cmd received:%f, %f", msg->linear.x, msg->angular.z); // ROS_INFO("cmd received:%f, %f", msg->linear.x, msg->angular.z);
} }
void ScoutROSMessenger::GetCurrentMotionCmdForSim(double &linear, double &angular) 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; linear = current_twist_.linear.x;
angular = current_twist_.angular.z; angular = current_twist_.angular.z;
} }
void ScoutROSMessenger::LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg) void ScoutROSMessenger::LightCmdCallback(
{ const scout_msgs::ScoutLightCmd::ConstPtr &msg) {
if (!simulated_robot_) if (!simulated_robot_) {
{ if (msg->enable_cmd_light_control) {
if (msg->enable_cmd_light_control)
{
ScoutLightCmd cmd; ScoutLightCmd cmd;
switch (msg->front_mode) switch (msg->front_mode) {
{ case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: {
case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
{
cmd.front_mode = ScoutLightCmd::LightMode::CONST_OFF; cmd.front_mode = ScoutLightCmd::LightMode::CONST_OFF;
break; break;
} }
case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: {
{
cmd.front_mode = ScoutLightCmd::LightMode::CONST_ON; cmd.front_mode = ScoutLightCmd::LightMode::CONST_ON;
break; break;
} }
case scout_msgs::ScoutLightCmd::LIGHT_BREATH: case scout_msgs::ScoutLightCmd::LIGHT_BREATH: {
{
cmd.front_mode = ScoutLightCmd::LightMode::BREATH; cmd.front_mode = ScoutLightCmd::LightMode::BREATH;
break; break;
} }
case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: {
{
cmd.front_mode = ScoutLightCmd::LightMode::CUSTOM; cmd.front_mode = ScoutLightCmd::LightMode::CUSTOM;
cmd.front_custom_value = msg->front_custom_value; cmd.front_custom_value = msg->front_custom_value;
break; break;
} }
} }
switch (msg->rear_mode) switch (msg->rear_mode) {
{ case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: {
case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
{
cmd.rear_mode = ScoutLightCmd::LightMode::CONST_OFF; cmd.rear_mode = ScoutLightCmd::LightMode::CONST_OFF;
break; break;
} }
case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: {
{
cmd.rear_mode = ScoutLightCmd::LightMode::CONST_ON; cmd.rear_mode = ScoutLightCmd::LightMode::CONST_ON;
break; break;
} }
case scout_msgs::ScoutLightCmd::LIGHT_BREATH: case scout_msgs::ScoutLightCmd::LIGHT_BREATH: {
{
cmd.rear_mode = ScoutLightCmd::LightMode::BREATH; cmd.rear_mode = ScoutLightCmd::LightMode::BREATH;
break; break;
} }
case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: {
{
cmd.rear_mode = ScoutLightCmd::LightMode::CUSTOM; cmd.rear_mode = ScoutLightCmd::LightMode::CUSTOM;
cmd.rear_custom_value = msg->rear_custom_value; cmd.rear_custom_value = msg->rear_custom_value;
break; break;
@@ -114,26 +98,20 @@ void ScoutROSMessenger::LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstP
} }
scout_->SetLightCommand(cmd); scout_->SetLightCommand(cmd);
} } else {
else
{
scout_->DisableLightCmdControl(); scout_->DisableLightCmdControl();
} }
} } else {
else
{
std::cout << "simulated robot received light control cmd" << std::endl; std::cout << "simulated robot received light control cmd" << std::endl;
} }
} }
void ScoutROSMessenger::PublishStateToROS() void ScoutROSMessenger::PublishStateToROS() {
{
current_time_ = ros::Time::now(); current_time_ = ros::Time::now();
double dt = (current_time_ - last_time_).toSec(); double dt = (current_time_ - last_time_).toSec();
static bool init_run = true; static bool init_run = true;
if (init_run) if (init_run) {
{
last_time_ = current_time_; last_time_ = current_time_;
init_run = false; init_run = false;
return; return;
@@ -154,8 +132,7 @@ void ScoutROSMessenger::PublishStateToROS()
status_msg.fault_code = state.fault_code; status_msg.fault_code = state.fault_code;
status_msg.battery_voltage = state.battery_voltage; status_msg.battery_voltage = state.battery_voltage;
for (int i = 0; i < 4; ++i) for (int i = 0; i < 4; ++i) {
{
status_msg.motor_states[i].current = state.motor_states[i].current; 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].rpm = state.motor_states[i].rpm;
status_msg.motor_states[i].temperature = state.motor_states[i].temperature; status_msg.motor_states[i].temperature = state.motor_states[i].temperature;
@@ -163,9 +140,11 @@ void ScoutROSMessenger::PublishStateToROS()
status_msg.light_control_enabled = state.light_control_enabled; status_msg.light_control_enabled = state.light_control_enabled;
status_msg.front_light_state.mode = state.front_light_state.mode; 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.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.mode = state.rear_light_state.mode;
status_msg.rear_light_state.custom_value = state.front_light_state.custom_value; status_msg.rear_light_state.custom_value =
state.front_light_state.custom_value;
status_publisher_.publish(status_msg); status_publisher_.publish(status_msg);
@@ -176,15 +155,13 @@ void ScoutROSMessenger::PublishStateToROS()
last_time_ = current_time_; last_time_ = current_time_;
} }
void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) {
{
current_time_ = ros::Time::now(); current_time_ = ros::Time::now();
double dt = (current_time_ - last_time_).toSec(); double dt = (current_time_ - last_time_).toSec();
static bool init_run = true; static bool init_run = true;
if (init_run) if (init_run) {
{
last_time_ = current_time_; last_time_ = current_time_;
init_run = false; init_run = false;
return; return;
@@ -207,14 +184,16 @@ void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular)
// { // {
// status_msg.motor_states[i].current = state.motor_states[i].current; // 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].rpm = state.motor_states[i].rpm;
// status_msg.motor_states[i].temperature = state.motor_states[i].temperature; // status_msg.motor_states[i].temperature =
// state.motor_states[i].temperature;
// } // }
status_msg.light_control_enabled = false; status_msg.light_control_enabled = false;
// status_msg.front_light_state.mode = state.front_light_state.mode; // 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.front_light_state.custom_value =
// status_msg.rear_light_state.mode = state.rear_light_state.mode; // state.front_light_state.custom_value; status_msg.rear_light_state.mode =
// status_msg.rear_light_state.custom_value = state.front_light_state.custom_value; // state.rear_light_state.mode; status_msg.rear_light_state.custom_value =
// state.front_light_state.custom_value;
status_publisher_.publish(status_msg); status_publisher_.publish(status_msg);
@@ -225,8 +204,8 @@ void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular)
last_time_ = current_time_; last_time_ = current_time_;
} }
void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular, double dt) void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular,
{ double dt) {
// perform numerical integration to get an estimation of pose // perform numerical integration to get an estimation of pose
linear_speed_ = linear; linear_speed_ = linear;
angular_speed_ = angular; angular_speed_ = angular;