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 to use next branch of ugv_sdk

Browse Source
This commit is contained in:
Ruixiang Du
2021-10-05 15:04:21 +08:00
parent fd7ae717cb
commit 117f8def26
4 changed files with 308 additions and 322 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -11,6 +11,7 @@
#define SCOUT_MESSENGER_HPP #define SCOUT_MESSENGER_HPP
#include <string> #include <string>
#include <mutex>
#include <ros/ros.h> #include <ros/ros.h>
#include <nav_msgs/Odometry.h> #include <nav_msgs/Odometry.h>
@@ -18,15 +19,13 @@
#include <tf2_ros/transform_broadcaster.h> #include <tf2_ros/transform_broadcaster.h>
#include "scout_msgs/ScoutLightCmd.h" #include "scout_msgs/ScoutLightCmd.h"
#include "ugv_sdk/scout_base.hpp" #include "ugv_sdk/mobile_robot/scout_robot.hpp"
namespace westonrobot namespace westonrobot {
{ class ScoutROSMessenger {
class ScoutROSMessenger public:
{
public:
explicit ScoutROSMessenger(ros::NodeHandle *nh); explicit ScoutROSMessenger(ros::NodeHandle *nh);
ScoutROSMessenger(ScoutBase *scout, ros::NodeHandle *nh); ScoutROSMessenger(std::shared_ptr<ScoutRobot> scout, ros::NodeHandle *nh);
std::string odom_frame_; std::string odom_frame_;
std::string base_frame_; std::string base_frame_;
@@ -42,8 +41,8 @@ public:
void GetCurrentMotionCmdForSim(double &linear, double &angular); void GetCurrentMotionCmdForSim(double &linear, double &angular);
private: private:
ScoutBase *scout_; std::shared_ptr<ScoutRobot> scout_;
ros::NodeHandle *nh_; ros::NodeHandle *nh_;
std::mutex twist_mutex_; std::mutex twist_mutex_;

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

@@ -10,15 +10,15 @@
#ifndef SCOUT_PARAMS_HPP #ifndef SCOUT_PARAMS_HPP
#define SCOUT_PARAMS_HPP #define SCOUT_PARAMS_HPP
namespace westonrobot namespace westonrobot {
{ struct ScoutParams {
struct ScoutParams
{
/* Scout Parameters */ /* Scout Parameters */
static constexpr double max_steer_angle = 30.0; // in degree 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 track =
static constexpr double wheelbase = 0.498; // in meter (front & rear wheel distance) 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 static constexpr double wheel_radius = 0.165; // in meter
// from user manual v1.2.8 P18 // from user manual v1.2.8 P18
@@ -27,7 +27,7 @@ namespace westonrobot
static constexpr double max_linear_speed = 1.5; // in m/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_angular_speed = 0.7853; // in rad/s
static constexpr double max_speed_cmd = 10.0; // in rad/s static constexpr double max_speed_cmd = 10.0; // in rad/s
}; };
} // namespace westonrobot } // namespace westonrobot
#endif /* SCOUT_PARAMS_HPP */ #endif /* SCOUT_PARAMS_HPP */

9
scout_base/src/scout_base_node.cpp
View File

@@ -1,16 +1,17 @@
#include <memory> #include <memory>
#include <csignal>
#include <ros/ros.h> #include <ros/ros.h>
#include <nav_msgs/Odometry.h> #include <nav_msgs/Odometry.h>
#include <sensor_msgs/JointState.h> #include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h> #include <tf/transform_broadcaster.h>
#include "ugv_sdk/scout_base.hpp" #include "ugv_sdk/mobile_robot/scout_robot.hpp"
#include "scout_base/scout_messenger.hpp" #include "scout_base/scout_messenger.hpp"
using namespace westonrobot; using namespace westonrobot;
std::shared_ptr<ScoutBase> robot; std::shared_ptr<ScoutRobot> robot;
bool keep_run = true; bool keep_run = true;
void DetachRobot(int signal) { keep_run = false; } void DetachRobot(int signal) { keep_run = false; }
@@ -23,8 +24,8 @@ int main(int argc, char **argv) {
std::signal(SIGINT, DetachRobot); std::signal(SIGINT, DetachRobot);
// instantiate a robot object // instantiate a robot object
robot = std::make_shared<ScoutBase>(); robot = std::make_shared<ScoutRobot>();
ScoutROSMessenger messenger(robot.get(), &node); ScoutROSMessenger messenger(robot, &node);
// fetch parameters before connecting to robot // fetch parameters before connecting to robot
std::string port_name; std::string port_name;

202
scout_base/src/scout_messenger.cpp
View File

@@ -13,16 +13,15 @@
#include "scout_msgs/ScoutStatus.h" #include "scout_msgs/ScoutStatus.h"
namespace westonrobot namespace westonrobot {
{ ScoutROSMessenger::ScoutROSMessenger(ros::NodeHandle *nh)
ScoutROSMessenger::ScoutROSMessenger(ros::NodeHandle *nh)
: scout_(nullptr), nh_(nh) {} : scout_(nullptr), nh_(nh) {}
ScoutROSMessenger::ScoutROSMessenger(ScoutBase *scout, ros::NodeHandle *nh) ScoutROSMessenger::ScoutROSMessenger(std::shared_ptr<ScoutRobot> scout,
ros::NodeHandle *nh)
: scout_(scout), nh_(nh) {} : scout_(scout), nh_(nh) {}
void ScoutROSMessenger::SetupSubscription() void ScoutROSMessenger::SetupSubscription() {
{
// odometry publisher // odometry publisher
odom_publisher_ = nh_->advertise<nav_msgs::Odometry>(odom_topic_name_, 50); odom_publisher_ = nh_->advertise<nav_msgs::Odometry>(odom_topic_name_, 50);
status_publisher_ = status_publisher_ =
@@ -33,117 +32,98 @@ namespace westonrobot
"/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this); "/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this);
light_cmd_subscriber_ = nh_->subscribe<scout_msgs::ScoutLightCmd>( light_cmd_subscriber_ = nh_->subscribe<scout_msgs::ScoutLightCmd>(
"/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this); "/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this);
} }
void ScoutROSMessenger::TwistCmdCallback( void ScoutROSMessenger::TwistCmdCallback(
const geometry_msgs::Twist::ConstPtr &msg) const geometry_msgs::Twist::ConstPtr &msg) {
{ if (!simulated_robot_) {
if (!simulated_robot_)
{
scout_->SetMotionCommand(msg->linear.x, msg->angular.z); scout_->SetMotionCommand(msg->linear.x, msg->angular.z);
} } else {
else
{
std::lock_guard<std::mutex> guard(twist_mutex_); std::lock_guard<std::mutex> guard(twist_mutex_);
current_twist_ = *msg.get(); 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, void ScoutROSMessenger::GetCurrentMotionCmdForSim(double &linear,
double &angular) double &angular) {
{
std::lock_guard<std::mutex> guard(twist_mutex_); std::lock_guard<std::mutex> guard(twist_mutex_);
linear = current_twist_.linear.x; linear = current_twist_.linear.x;
angular = current_twist_.angular.z; angular = current_twist_.angular.z;
} }
void ScoutROSMessenger::LightCmdCallback( void ScoutROSMessenger::LightCmdCallback(
const scout_msgs::ScoutLightCmd::ConstPtr &msg) const scout_msgs::ScoutLightCmd::ConstPtr &msg) {
{ if (!simulated_robot_) {
if (!simulated_robot_) // if (msg->cmd_ctrl_allowed) {
{ // ScoutLightCmd cmd;
if (msg->cmd_ctrl_allowed)
{
ScoutLightCmd cmd;
switch (msg->front_mode) // switch (msg->front_mode) {
{ // case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: {
case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: // cmd.front_mode = CONST_OFF;
{
cmd.front_mode = CONST_OFF;
break;
}
case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
{
cmd.front_mode = CONST_ON;
break;
}
case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
{
cmd.front_mode = BREATH;
break;
}
case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
{
cmd.front_mode = CUSTOM;
cmd.front_custom_value = msg->front_custom_value;
break;
}
}
// not meant to be controlled by user for now
// switch (msg->rear_mode)
// {
// case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
// {
// cmd.rear_mode = CONST_OFF;
// break; // break;
// } // }
// case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: // case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: {
// { // cmd.front_mode = CONST_ON;
// cmd.rear_mode = CONST_ON;
// break; // break;
// } // }
// case scout_msgs::ScoutLightCmd::LIGHT_BREATH: // case scout_msgs::ScoutLightCmd::LIGHT_BREATH: {
// { // cmd.front_mode = BREATH;
// cmd.rear_mode = BREATH;
// break; // break;
// } // }
// case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: // case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: {
// { // cmd.front_mode = CUSTOM;
// cmd.rear_mode = CUSTOM; // cmd.front_custom_value = msg->front_custom_value;
// cmd.rear_custom_value = msg->rear_custom_value;
// break; // break;
// } // }
// } // }
cmd.cmd_ctrl_allowed = true; // // not meant to be controlled by user for now
scout_->SetLightCommand(cmd); // // switch (msg->rear_mode)
} // // {
else // // case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
{ // // {
scout_->DisableLightControl(); // // cmd.rear_mode = CONST_OFF;
} // // break;
} // // }
else // // case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
{ // // {
// // cmd.rear_mode = CONST_ON;
// // break;
// // }
// // case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
// // {
// // cmd.rear_mode = BREATH;
// // break;
// // }
// // case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
// // {
// // cmd.rear_mode = CUSTOM;
// // cmd.rear_custom_value = msg->rear_custom_value;
// // break;
// // }
// // }
// cmd.cmd_ctrl_allowed = true;
// scout_->SetLightCommand(cmd);
// } else {
// scout_->DisableLightControl();
// }
} else {
std::cout << "simulated robot received light control cmd" << std::endl; std::cout << "simulated robot received light control cmd" << std::endl;
} }
} }
void ScoutROSMessenger::PublishStateToROS() void ScoutROSMessenger::PublishStateToROS() {
{
current_time_ = ros::Time::now(); current_time_ = ros::Time::now();
double dt = (current_time_ - last_time_).toSec(); double dt = (current_time_ - last_time_).toSec();
static bool init_run = true; static bool init_run = true;
if (init_run) if (init_run) {
{
last_time_ = current_time_; last_time_ = current_time_;
init_run = false; init_run = false;
return; return;
} }
auto state = scout_->GetScoutState(); auto state = scout_->GetRobotState();
// publish scout state message // publish scout state message
scout_msgs::ScoutStatus status_msg; scout_msgs::ScoutStatus status_msg;
@@ -158,25 +138,33 @@ namespace westonrobot
status_msg.error_code = state.system_state.error_code; status_msg.error_code = state.system_state.error_code;
status_msg.battery_voltage = state.system_state.battery_voltage; status_msg.battery_voltage = state.system_state.battery_voltage;
for (int i = 0; i < 4; ++i) auto actuator = scout_->GetActuatorState();
{
// actuator_hs_state
uint8_t motor_id = state.actuator_hs_state[i].motor_id;
status_msg.actuator_states[motor_id].rpm = state.actuator_hs_state[i].rpm; for (int i = 0; i < 4; ++i) {
status_msg.actuator_states[motor_id].current = state.actuator_hs_state[i].current; // actuator_hs_state
status_msg.actuator_states[motor_id].pulse_count = state.actuator_hs_state[i].pulse_count; uint8_t motor_id = actuator.actuator_hs_state[i].motor_id;
status_msg.actuator_states[motor_id].rpm =
actuator.actuator_hs_state[i].rpm;
status_msg.actuator_states[motor_id].current =
actuator.actuator_hs_state[i].current;
status_msg.actuator_states[motor_id].pulse_count =
actuator.actuator_hs_state[i].pulse_count;
// actuator_ls_state // actuator_ls_state
motor_id = state.actuator_ls_state[i].motor_id; motor_id = actuator.actuator_ls_state[i].motor_id;
status_msg.actuator_states[motor_id].driver_voltage = state.actuator_ls_state[i].driver_voltage; status_msg.actuator_states[motor_id].driver_voltage =
status_msg.actuator_states[motor_id].driver_temperature = state.actuator_ls_state[i].driver_temperature; actuator.actuator_ls_state[i].driver_voltage;
status_msg.actuator_states[motor_id].motor_temperature = state.actuator_ls_state[i].motor_temperature; status_msg.actuator_states[motor_id].driver_temperature =
status_msg.actuator_states[motor_id].driver_state = state.actuator_ls_state[i].driver_state; actuator.actuator_ls_state[i].driver_temp;
status_msg.actuator_states[motor_id].motor_temperature =
actuator.actuator_ls_state[i].motor_temp;
status_msg.actuator_states[motor_id].driver_state =
actuator.actuator_ls_state[i].driver_state;
} }
status_msg.light_control_enabled = state.light_state.cmd_ctrl_allowed; status_msg.light_control_enabled = state.light_state.enable_cmd_ctrl;
status_msg.front_light_state.mode = state.light_state.front_light.mode; status_msg.front_light_state.mode = state.light_state.front_light.mode;
status_msg.front_light_state.custom_value = status_msg.front_light_state.custom_value =
state.light_state.front_light.custom_value; state.light_state.front_light.custom_value;
@@ -186,21 +174,20 @@ namespace westonrobot
status_publisher_.publish(status_msg); status_publisher_.publish(status_msg);
// publish odometry and tf // publish odometry and tf
PublishOdometryToROS(state.motion_state.linear_velocity, state.motion_state.angular_velocity, dt); PublishOdometryToROS(state.motion_state.linear_velocity,
state.motion_state.angular_velocity, dt);
// record time for next integration // record time for next integration
last_time_ = current_time_; last_time_ = current_time_;
} }
void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) {
{
current_time_ = ros::Time::now(); current_time_ = ros::Time::now();
double dt = (current_time_ - last_time_).toSec(); double dt = (current_time_ - last_time_).toSec();
static bool init_run = true; static bool init_run = true;
if (init_run) if (init_run) {
{
last_time_ = current_time_; last_time_ = current_time_;
init_run = false; init_run = false;
return; return;
@@ -241,11 +228,10 @@ namespace westonrobot
// record time for next integration // record time for next integration
last_time_ = current_time_; last_time_ = current_time_;
} }
void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular, void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular,
double dt) double dt) {
{
// perform numerical integration to get an estimation of pose // perform numerical integration to get an estimation of pose
linear_speed_ = linear; linear_speed_ = linear;
angular_speed_ = angular; angular_speed_ = angular;
@@ -289,5 +275,5 @@ namespace westonrobot
odom_msg.twist.twist.angular.z = angular_speed_; odom_msg.twist.twist.angular.z = angular_speed_;
odom_publisher_.publish(odom_msg); odom_publisher_.publish(odom_msg);
} }
} // namespace westonrobot } // namespace westonrobot