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

messenger: cleaned up messenger interface

Browse Source
This commit is contained in:
Ruixiang Du
2021-10-05 16:54:36 +08:00
parent 117f8def26
commit 7d9f027137
5 changed files with 172 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files

95
scout_base/src/scout_base_node.cpp
View File

@@ -7,14 +7,17 @@
#include <tf/transform_broadcaster.h>
#include "ugv_sdk/mobile_robot/scout_robot.hpp"
#include "ugv_sdk/utilities/protocol_detector.hpp"
#include "scout_base/scout_messenger.hpp"
using namespace westonrobot;
std::shared_ptr<ScoutRobot> robot;
std::unique_ptr<ScoutMessenger> messenger;
bool keep_run = true;
void DetachRobot(int signal) { keep_run = false; }
void DetachRobot(int signal) { messenger->Stop(); }
int main(int argc, char **argv) {
// setup ROS node
@@ -23,46 +26,72 @@ int main(int argc, char **argv) {
std::signal(SIGINT, DetachRobot);
// instantiate a robot object
robot = std::make_shared<ScoutRobot>();
ScoutROSMessenger messenger(robot, &node);
// fetch parameters before connecting to robot
std::string port_name;
private_node.param<std::string>("port_name", port_name, std::string("can0"));
private_node.param<std::string>("odom_frame", messenger.odom_frame_,
std::string("odom"));
private_node.param<std::string>("base_frame", messenger.base_frame_,
std::string("base_link"));
private_node.param<bool>("simulated_robot", messenger.simulated_robot_,
false);
private_node.param<int>("control_rate", messenger.sim_control_rate_, 50);
private_node.param<std::string>("odom_topic_name", messenger.odom_topic_name_,
std::string("odom"));
std::string odom_frame;
std::string base_frame;
std::string odom_topic_name;
bool is_simulated = false;
int sim_rate = 50;
bool is_scout_mini = false;
if (!messenger.simulated_robot_) {
// connect to robot and setup ROS subscription
if (port_name.find("can") != std::string::npos) {
robot->Connect(port_name);
private_node.param<std::string>("port_name", port_name, std::string("can0"));
private_node.param<std::string>("odom_frame", odom_frame,
std::string("odom"));
private_node.param<std::string>("base_frame", base_frame,
std::string("base_link"));
private_node.param<bool>("simulated_robot", is_simulated, false);
private_node.param<int>("control_rate", sim_rate, 50);
private_node.param<std::string>("odom_topic_name", odom_topic_name,
std::string("odom"));
private_node.param<bool>("is_scout_mini", is_scout_mini, false);
if (is_scout_mini) {
ROS_INFO("Robot base: Scout Mini");
} else {
ROS_INFO("Robot base: Scout");
}
// instantiate a robot object
ProtocolDectctor detector;
if (detector.Connect(port_name)) {
auto proto = detector.DetectProtocolVersion(5);
if (proto == ProtocolVersion::AGX_V1) {
std::cout << "Detected protocol: AGX_V1" << std::endl;
robot =
std::make_shared<ScoutRobot>(ProtocolVersion::AGX_V1, is_scout_mini);
} else if (proto == ProtocolVersion::AGX_V2) {
std::cout << "Detected protocol: AGX_V2" << std::endl;
robot =
std::make_shared<ScoutRobot>(ProtocolVersion::AGX_V2, is_scout_mini);
} else {
std::cout << "Detected protocol: UNKONWN" << std::endl;
return -1;
}
} else {
return -1;
}
// instantiate a ROS messenger
messenger = std::unique_ptr<ScoutMessenger>(new ScoutMessenger(robot, &node));
messenger->SetOdometryFrame(odom_frame);
messenger->SetBaseFrame(base_frame);
messenger->SetOdometryTopicName(odom_topic_name);
if (is_simulated) messenger->SetSimulationMode(sim_rate);
// connect to robot and setup ROS subscription
if (port_name.find("can") != std::string::npos) {
if (robot->Connect(port_name)) {
robot->EnableCommandedMode();
ROS_INFO("Using CAN bus to talk with the robot");
}
}
messenger.SetupSubscription();
// publish robot state at 50Hz while listening to twist commands
ros::Rate rate(50);
while (keep_run) {
if (!messenger.simulated_robot_) {
messenger.PublishStateToROS();
} else {
double linear, angular;
messenger.GetCurrentMotionCmdForSim(linear, angular);
messenger.PublishSimStateToROS(linear, angular);
ROS_INFO("Failed to connect to the robot CAN bus");
return -1;
}
ros::spinOnce();
rate.sleep();
}
messenger->Run();
return 0;
}

81
scout_base/src/scout_messenger.cpp
View File

@@ -14,27 +14,56 @@
#include "scout_msgs/ScoutStatus.h"
namespace westonrobot {
ScoutROSMessenger::ScoutROSMessenger(ros::NodeHandle *nh)
ScoutMessenger::ScoutMessenger(ros::NodeHandle *nh)
: scout_(nullptr), nh_(nh) {}
ScoutROSMessenger::ScoutROSMessenger(std::shared_ptr<ScoutRobot> scout,
ros::NodeHandle *nh)
ScoutMessenger::ScoutMessenger(std::shared_ptr<ScoutRobot> scout,
ros::NodeHandle *nh)
: scout_(scout), nh_(nh) {}
void ScoutROSMessenger::SetupSubscription() {
// odometry publisher
odom_publisher_ = nh_->advertise<nav_msgs::Odometry>(odom_topic_name_, 50);
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);
light_cmd_subscriber_ = nh_->subscribe<scout_msgs::ScoutLightCmd>(
"/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this);
void ScoutMessenger::SetOdometryFrame(std::string frame) {
odom_frame_ = frame;
}
void ScoutROSMessenger::TwistCmdCallback(
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);
@@ -42,17 +71,17 @@ void ScoutROSMessenger::TwistCmdCallback(
std::lock_guard<std::mutex> guard(twist_mutex_);
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 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 ScoutROSMessenger::LightCmdCallback(
void ScoutMessenger::LightCmdCallback(
const scout_msgs::ScoutLightCmd::ConstPtr &msg) {
if (!simulated_robot_) {
// if (msg->cmd_ctrl_allowed) {
@@ -112,7 +141,7 @@ void ScoutROSMessenger::LightCmdCallback(
}
}
void ScoutROSMessenger::PublishStateToROS() {
void ScoutMessenger::PublishStateToROS() {
current_time_ = ros::Time::now();
double dt = (current_time_ - last_time_).toSec();
@@ -171,7 +200,7 @@ void ScoutROSMessenger::PublishStateToROS() {
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);
status_pub_.publish(status_msg);
// publish odometry and tf
PublishOdometryToROS(state.motion_state.linear_velocity,
@@ -181,7 +210,7 @@ void ScoutROSMessenger::PublishStateToROS() {
last_time_ = current_time_;
}
void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) {
void ScoutMessenger::PublishSimStateToROS(double linear, double angular) {
current_time_ = ros::Time::now();
double dt = (current_time_ - last_time_).toSec();
@@ -221,7 +250,7 @@ void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) {
// state.rear_light_state.mode; status_msg.rear_light_state.custom_value =
// state.front_light_state.custom_value;
status_publisher_.publish(status_msg);
status_pub_.publish(status_msg);
// publish odometry and tf
PublishOdometryToROS(linear, angular, dt);
@@ -230,8 +259,8 @@ void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) {
last_time_ = current_time_;
}
void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular,
double dt) {
void ScoutMessenger::PublishOdometryToROS(double linear, double angular,
double dt) {
// perform numerical integration to get an estimation of pose
linear_speed_ = linear;
angular_speed_ = angular;
@@ -274,6 +303,6 @@ void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular,
odom_msg.twist.twist.linear.y = 0.0;
odom_msg.twist.twist.angular.z = angular_speed_;
odom_publisher_.publish(odom_msg);
odom_pub_.publish(odom_msg);
}
} // namespace westonrobot