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

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"
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(ScoutBase *scout, ros::NodeHandle *nh)
: scout_(scout), nh_(nh) {}
void ScoutROSMessenger::SetupSubscription()
{
void ScoutROSMessenger::SetupSubscription() {
// odometry publisher
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
motion_cmd_subscriber_ = nh_->subscribe<geometry_msgs::Twist>("/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this); //不启用平滑包则订阅“cmd_vel”
light_cmd_subscriber_ = nh_->subscribe<scout_msgs::ScoutLightCmd>("/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this);
motion_cmd_subscriber_ = nh_->subscribe<geometry_msgs::Twist>(
"/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)
{
if (!simulated_robot_)
{
void ScoutROSMessenger::TwistCmdCallback(
const geometry_msgs::Twist::ConstPtr &msg) {
if (!simulated_robot_) {
scout_->SetMotionCommand(msg->linear.x, msg->angular.z);
}
else
{
} 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)
{
void ScoutROSMessenger::GetCurrentMotionCmdForSim(double &linear,
double &angular) {
std::lock_guard<std::mutex> guard(twist_mutex_);
linear = current_twist_.linear.x;
angular = current_twist_.angular.z;
}
void ScoutROSMessenger::LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg)
{
if (!simulated_robot_)
{
if (msg->enable_cmd_light_control)
{
void ScoutROSMessenger::LightCmdCallback(
const scout_msgs::ScoutLightCmd::ConstPtr &msg) {
if (!simulated_robot_) {
if (msg->enable_cmd_light_control) {
ScoutLightCmd cmd;
switch (msg->front_mode)
{
case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
{
switch (msg->front_mode) {
case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: {
cmd.front_mode = ScoutLightCmd::LightMode::CONST_OFF;
break;
}
case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
{
case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: {
cmd.front_mode = ScoutLightCmd::LightMode::CONST_ON;
break;
}
case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
{
case scout_msgs::ScoutLightCmd::LIGHT_BREATH: {
cmd.front_mode = ScoutLightCmd::LightMode::BREATH;
break;
}
case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
{
case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: {
cmd.front_mode = ScoutLightCmd::LightMode::CUSTOM;
cmd.front_custom_value = msg->front_custom_value;
break;
}
}
switch (msg->rear_mode)
{
case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
{
switch (msg->rear_mode) {
case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: {
cmd.rear_mode = ScoutLightCmd::LightMode::CONST_OFF;
break;
}
case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
{
case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: {
cmd.rear_mode = ScoutLightCmd::LightMode::CONST_ON;
break;
}
case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
{
case scout_msgs::ScoutLightCmd::LIGHT_BREATH: {
cmd.rear_mode = ScoutLightCmd::LightMode::BREATH;
break;
}
case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
{
case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: {
cmd.rear_mode = ScoutLightCmd::LightMode::CUSTOM;
cmd.rear_custom_value = msg->rear_custom_value;
break;
@@ -114,26 +98,20 @@ void ScoutROSMessenger::LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstP
}
scout_->SetLightCommand(cmd);
}
else
{
} else {
scout_->DisableLightCmdControl();
}
}
else
{
} else {
std::cout << "simulated robot received light control cmd" << std::endl;
}
}
void ScoutROSMessenger::PublishStateToROS()
{
void ScoutROSMessenger::PublishStateToROS() {
current_time_ = ros::Time::now();
double dt = (current_time_ - last_time_).toSec();
static bool init_run = true;
if (init_run)
{
if (init_run) {
last_time_ = current_time_;
init_run = false;
return;
@@ -154,8 +132,7 @@ void ScoutROSMessenger::PublishStateToROS()
status_msg.fault_code = state.fault_code;
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].rpm = state.motor_states[i].rpm;
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.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.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);
@@ -176,15 +155,13 @@ void ScoutROSMessenger::PublishStateToROS()
last_time_ = current_time_;
}
void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular)
{
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)
{
if (init_run) {
last_time_ = current_time_;
init_run = false;
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].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.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_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);
@@ -225,8 +204,8 @@ void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular)
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
linear_speed_ = linear;
angular_speed_ = angular;