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

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

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

@@ -22,10 +22,22 @@
#include "ugv_sdk/mobile_robot/scout_robot.hpp"
namespace westonrobot {
class ScoutROSMessenger {
class ScoutMessenger {
public:
explicit ScoutROSMessenger(ros::NodeHandle *nh);
ScoutROSMessenger(std::shared_ptr<ScoutRobot> scout, ros::NodeHandle *nh);
explicit ScoutMessenger(ros::NodeHandle *nh);
ScoutMessenger(std::shared_ptr<ScoutRobot> scout, ros::NodeHandle *nh);
void SetOdometryFrame(std::string frame);
void SetBaseFrame(std::string frame);
void SetOdometryTopicName(std::string name);
void SetSimulationMode(int loop_rate);
void Run();
void Stop();
private:
std::shared_ptr<ScoutRobot> scout_;
ros::NodeHandle *nh_;
std::string odom_frame_;
std::string base_frame_;
@@ -34,29 +46,20 @@ class ScoutROSMessenger {
bool simulated_robot_ = false;
int sim_control_rate_ = 50;
void SetupSubscription();
void PublishStateToROS();
void PublishSimStateToROS(double linear, double angular);
void GetCurrentMotionCmdForSim(double &linear, double &angular);
private:
std::shared_ptr<ScoutRobot> scout_;
ros::NodeHandle *nh_;
std::atomic<bool> keep_running_;
std::mutex twist_mutex_;
geometry_msgs::Twist current_twist_;
ros::Publisher odom_publisher_;
ros::Publisher status_publisher_;
ros::Subscriber motion_cmd_subscriber_;
ros::Subscriber light_cmd_subscriber_;
ros::Publisher odom_pub_;
ros::Publisher status_pub_;
ros::Subscriber motion_cmd_sub_;
ros::Subscriber light_cmd_sub_;
tf2_ros::TransformBroadcaster tf_broadcaster_;
// speed variables
double linear_speed_ = 0.0;
double angular_speed_ = 0.0;
double position_x_ = 0.0;
double position_y_ = 0.0;
double theta_ = 0.0;
@@ -64,9 +67,15 @@ class ScoutROSMessenger {
ros::Time last_time_;
ros::Time current_time_;
void SetupSubscription();
void TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg);
void LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg);
void PublishOdometryToROS(double linear, double angular, double dt);
void PublishStateToROS();
void PublishSimStateToROS(double linear, double angular);
void GetCurrentMotionCmdForSim(double &linear, double &angular);
};
} // namespace westonrobot

33
scout_base/include/scout_base/scout_params.hpp
View File

@@ -1,33 +0,0 @@
/*
* scout_params.hpp
*
* Created on: Sep 27, 2019 15:08
* Description:
*
* Copyright (c) 2020 Ruixiang Du (rdu)
*/
#ifndef SCOUT_PARAMS_HPP
#define SCOUT_PARAMS_HPP
namespace westonrobot {
struct ScoutParams {
/* Scout Parameters */
static constexpr double max_steer_angle = 30.0; // in degree
static constexpr double track =
0.58306; // in meter (left & right wheel distance)
static constexpr double wheelbase =
0.498; // in meter (front & rear wheel distance)
static constexpr double wheel_radius = 0.165; // in meter
// from user manual v1.2.8 P18
// max linear velocity: 1.5 m/s
// max angular velocity: 0.7853 rad/s
static constexpr double max_linear_speed = 1.5; // in m/s
static constexpr double max_angular_speed = 0.7853; // in rad/s
static constexpr double max_speed_cmd = 10.0; // in rad/s
};
} // namespace westonrobot
#endif /* SCOUT_PARAMS_HPP */

28
scout_base/launch/scout_mini_omni_base.launch Normal file
View File

@@ -0,0 +1,28 @@
<launch>
<!--
The robot can be controlled either through CAN bus or UART port. Make sure the hardware
interface is set up correctly before attempting to connect to the robot.
You only need to specify the port name, such as "can0", "/dev/ttyUSB0". The port should
operate with the following configuration:
* CAN bus: 500k
* UART: 115200, Parity None, 8-bit Data, 1 Stop Bit
-->
<arg name="port_name" default="can0" />
<arg name="is_scout_mini" default="true" />
<arg name="is_omni_wheel" default="true" />
<arg name="simulated_robot" default="false" />
<arg name="odom_topic_name" default="odom" />
<node name="scout_base_node" pkg="scout_base" type="scout_base_node" output="screen">
<param name="is_scout_mini" type="bool" value="$(arg is_scout_mini)" />
<param name="is_omni_wheel" type="bool" value="$(arg is_omni_wheel)" />
<param name="port_name" type="string" value="$(arg port_name)" />
<param name="simulated_robot" type="bool" value="$(arg simulated_robot)" />
<param name="odom_frame" type="string" value="odom" />
<param name="base_frame" type="string" value="base_link" />
<param name="odom_topic_name" type="string" value="$(arg odom_topic_name)" />
</node>
</launch>

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