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

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

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;
}