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

updated template to support scout/scout mini/scout mini omni with v1 and v2 protocol

Browse Source
This commit is contained in:
Ruixiang Du
2021-10-05 19:01:10 +08:00
parent 27f4e5d411
commit 266d68d5fe
6 changed files with 117 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
scout_base/CMakeLists.txt
View File

@@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3)
project(scout_base)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
add_compile_options(-std=c++14)
find_package(catkin REQUIRED COMPONENTS
roslaunch

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

@@ -15,11 +15,11 @@
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
// #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 {
@@ -71,9 +71,6 @@ class ScoutMessenger {
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;
@@ -95,7 +92,7 @@ class ScoutMessenger {
void TwistCmdCallback(const geometry_msgs::Twist::ConstPtr &msg) {
if (!simulated_robot_) {
scout_->SetMotionCommand(msg->linear.x, msg->angular.z);
SetScoutMotionCommand(scout_, msg);
} else {
std::lock_guard<std::mutex> guard(twist_mutex_);
current_twist_ = *msg.get();
@@ -103,6 +100,22 @@ class ScoutMessenger {
ROS_INFO("Cmd received:%f, %f", msg->linear.x, msg->angular.z);
}
template <typename T,
std::enable_if_t<!std::is_base_of<ScoutMiniOmniRobot, T>::value,
bool> = true>
void SetScoutMotionCommand(std::shared_ptr<T> base,
const geometry_msgs::Twist::ConstPtr &msg) {
base->SetMotionCommand(msg->linear.x, msg->angular.z);
}
template <typename T,
std::enable_if_t<std::is_base_of<ScoutMiniOmniRobot, T>::value,
bool> = true>
void SetScoutMotionCommand(std::shared_ptr<T> base,
const geometry_msgs::Twist::ConstPtr &msg) {
base->SetMotionCommand(msg->linear.x, msg->angular.z, msg->linear.y);
}
void LightCmdCallback(const scout_msgs::ScoutLightCmd::ConstPtr &msg) {
if (!simulated_robot_) {
// if (msg->cmd_ctrl_allowed) {
@@ -162,14 +175,21 @@ class ScoutMessenger {
}
}
void PublishOdometryToROS(double linear, double angular, double dt) {
void PublishOdometryToROS(const MotionStateMessage &msg, double dt) {
// perform numerical integration to get an estimation of pose
linear_speed_ = linear;
angular_speed_ = angular;
double linear_speed = msg.linear_velocity;
double angular_speed = msg.angular_velocity;
double lateral_speed = 0;
double d_x = linear_speed_ * std::cos(theta_) * dt;
double d_y = linear_speed_ * std::sin(theta_) * dt;
double d_theta = angular_speed_ * dt;
if (std::is_base_of<ScoutMiniOmniRobot, ScoutType>::value) {
lateral_speed = msg.lateral_velocity;
} else {
lateral_speed = 0;
}
double d_x = linear_speed * std::cos(theta_) * dt;
double d_y = (linear_speed * std::sin(theta_) + lateral_speed) * dt;
double d_theta = angular_speed * dt;
position_x_ += d_x;
position_y_ += d_y;
@@ -202,9 +222,9 @@ class ScoutMessenger {
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_msg.twist.twist.linear.x = linear_speed;
odom_msg.twist.twist.linear.y = lateral_speed;
odom_msg.twist.twist.angular.z = angular_speed;
odom_pub_.publish(odom_msg);
}
@@ -271,64 +291,11 @@ class ScoutMessenger {
status_pub_.publish(status_msg);
// publish odometry and tf
PublishOdometryToROS(state.motion_state.linear_velocity,
state.motion_state.angular_velocity, dt);
PublishOdometryToROS(state.motion_state, 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

2
scout_base/launch/scout_base.launch
View File

@@ -11,11 +11,13 @@
-->
<arg name="port_name" default="can0" />
<arg name="is_scout_mini" default="false" />
<arg name="is_omni_wheel" default="false" />
<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" />

2
scout_base/launch/scout_mini_base.launch
View File

@@ -11,11 +11,13 @@
-->
<arg name="port_name" default="can0" />
<arg name="is_scout_mini" default="true" />
<arg name="is_omni_wheel" default="false" />
<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" />

2
scout_base/launch/scout_mini_omni_base.launch
View File

@@ -11,7 +11,7 @@
-->
<arg name="port_name" default="can0" />
<arg name="is_scout_mini" default="true" />
<arg name="is_omni_wheel" default="true" />
<arg name="is_omni_wheel" default="true" />
<arg name="simulated_robot" default="false" />
<arg name="odom_topic_name" default="odom" />

107
scout_base/src/scout_base_node.cpp
View File

@@ -18,13 +18,12 @@ int main(int argc, char **argv) {
ros::init(argc, argv, "scout_node");
ros::NodeHandle node(""), private_node("~");
// std::signal(SIGINT, DetachRobot);
// fetch parameters before connecting to robot
std::string port_name;
std::string odom_frame;
std::string base_frame;
std::string odom_topic_name;
bool is_omni_wheel = false;
bool is_simulated = false;
int sim_rate = 50;
bool is_scout_mini = false;
@@ -34,6 +33,7 @@ int main(int argc, char **argv) {
std::string("odom"));
private_node.param<std::string>("base_frame", base_frame,
std::string("base_link"));
private_node.param<bool>("is_omni_wheel", is_omni_wheel, false);
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,
@@ -46,50 +46,95 @@ int main(int argc, char **argv) {
ROS_INFO("Robot base: Scout");
}
std::shared_ptr<ScoutRobot> robot;
std::unique_ptr<ScoutMessenger<ScoutRobot>> messenger;
if (!is_omni_wheel) {
// instantiate a robot object
std::shared_ptr<ScoutRobot> robot;
// instantiate a robot object
ProtocolDectctor detector;
if (detector.Connect(port_name)) {
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
std::unique_ptr<ScoutMessenger<ScoutRobot>> messenger =
std::unique_ptr<ScoutMessenger<ScoutRobot>>(
new ScoutMessenger<ScoutRobot>(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");
} else {
ROS_INFO("Failed to connect to the robot CAN bus");
return -1;
}
}
messenger->Run();
} else {
// instantiate a robot object
std::shared_ptr<ScoutMiniOmniRobot> robot;
ProtocolDectctor detector;
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);
robot = std::unique_ptr<ScoutMiniOmniRobot>(
new ScoutMiniOmniRobot(ProtocolVersion::AGX_V1));
} 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);
robot = std::unique_ptr<ScoutMiniOmniRobot>(
new ScoutMiniOmniRobot(ProtocolVersion::AGX_V2));
} else {
std::cout << "Detected protocol: UNKONWN" << std::endl;
return -1;
}
} else {
return -1;
}
// instantiate a ROS messenger
messenger = std::unique_ptr<ScoutMessenger<ScoutRobot>>(
new ScoutMessenger<ScoutRobot>(robot, &node));
// instantiate a ROS messenger
std::unique_ptr<ScoutMessenger<ScoutMiniOmniRobot>> messenger =
std::unique_ptr<ScoutMessenger<ScoutMiniOmniRobot>>(
new ScoutMessenger<ScoutMiniOmniRobot>(robot, &node));
messenger->SetOdometryFrame(odom_frame);
messenger->SetBaseFrame(base_frame);
messenger->SetOdometryTopicName(odom_topic_name);
if (is_simulated) messenger->SetSimulationMode(sim_rate);
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");
} else {
ROS_INFO("Failed to connect to the robot CAN bus");
return -1;
// 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");
} else {
ROS_INFO("Failed to connect to the robot CAN bus");
return -1;
}
}
}
messenger->Run();
messenger->Run();
}
return 0;
}