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

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

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

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

@@ -10,15 +10,15 @@
#ifndef SCOUT_PARAMS_HPP
#define SCOUT_PARAMS_HPP
namespace westonrobot
{
struct ScoutParams
{
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 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

9
scout_base/src/scout_base_node.cpp
View File

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

178
scout_base/src/scout_messenger.cpp
View File

@@ -13,16 +13,15 @@
#include "scout_msgs/ScoutStatus.h"
namespace westonrobot
{
namespace westonrobot {
ScoutROSMessenger::ScoutROSMessenger(ros::NodeHandle *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) {}
void ScoutROSMessenger::SetupSubscription()
{
void ScoutROSMessenger::SetupSubscription() {
// odometry publisher
odom_publisher_ = nh_->advertise<nav_msgs::Odometry>(odom_topic_name_, 50);
status_publisher_ =
@@ -36,14 +35,10 @@ namespace westonrobot
}
void ScoutROSMessenger::TwistCmdCallback(
const geometry_msgs::Twist::ConstPtr &msg)
{
if (!simulated_robot_)
{
const geometry_msgs::Twist::ConstPtr &msg) {
if (!simulated_robot_) {
scout_->SetMotionCommand(msg->linear.x, msg->angular.z);
}
else
{
} else {
std::lock_guard<std::mutex> guard(twist_mutex_);
current_twist_ = *msg.get();
}
@@ -51,99 +46,84 @@ namespace westonrobot
}
void ScoutROSMessenger::GetCurrentMotionCmdForSim(double &linear,
double &angular)
{
double &angular) {
std::lock_guard<std::mutex> guard(twist_mutex_);
linear = current_twist_.linear.x;
angular = current_twist_.angular.z;
}
void ScoutROSMessenger::LightCmdCallback(
const scout_msgs::ScoutLightCmd::ConstPtr &msg)
{
if (!simulated_robot_)
{
if (msg->cmd_ctrl_allowed)
{
ScoutLightCmd cmd;
const scout_msgs::ScoutLightCmd::ConstPtr &msg) {
if (!simulated_robot_) {
// if (msg->cmd_ctrl_allowed) {
// ScoutLightCmd cmd;
switch (msg->front_mode)
{
case scout_msgs::ScoutLightCmd::LIGHT_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;
// switch (msg->front_mode) {
// case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: {
// cmd.front_mode = CONST_OFF;
// break;
// }
// case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
// {
// cmd.rear_mode = CONST_ON;
// case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: {
// cmd.front_mode = CONST_ON;
// break;
// }
// case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
// {
// cmd.rear_mode = BREATH;
// case scout_msgs::ScoutLightCmd::LIGHT_BREATH: {
// cmd.front_mode = BREATH;
// break;
// }
// case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
// {
// cmd.rear_mode = CUSTOM;
// cmd.rear_custom_value = msg->rear_custom_value;
// case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: {
// cmd.front_mode = CUSTOM;
// cmd.front_custom_value = msg->front_custom_value;
// break;
// }
// }
cmd.cmd_ctrl_allowed = true;
scout_->SetLightCommand(cmd);
}
else
{
scout_->DisableLightControl();
}
}
else
{
// // 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;
// // }
// // 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;
}
}
void ScoutROSMessenger::PublishStateToROS()
{
void ScoutROSMessenger::PublishStateToROS() {
current_time_ = ros::Time::now();
double dt = (current_time_ - last_time_).toSec();
static bool init_run = true;
if (init_run)
{
if (init_run) {
last_time_ = current_time_;
init_run = false;
return;
}
auto state = scout_->GetScoutState();
auto state = scout_->GetRobotState();
// publish scout state message
scout_msgs::ScoutStatus status_msg;
@@ -158,25 +138,33 @@ namespace westonrobot
status_msg.error_code = state.system_state.error_code;
status_msg.battery_voltage = state.system_state.battery_voltage;
for (int i = 0; i < 4; ++i)
{
// actuator_hs_state
uint8_t motor_id = state.actuator_hs_state[i].motor_id;
auto actuator = scout_->GetActuatorState();
status_msg.actuator_states[motor_id].rpm = state.actuator_hs_state[i].rpm;
status_msg.actuator_states[motor_id].current = state.actuator_hs_state[i].current;
status_msg.actuator_states[motor_id].pulse_count = state.actuator_hs_state[i].pulse_count;
for (int i = 0; i < 4; ++i) {
// actuator_hs_state
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
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_temperature = state.actuator_ls_state[i].driver_temperature;
status_msg.actuator_states[motor_id].motor_temperature = state.actuator_ls_state[i].motor_temperature;
status_msg.actuator_states[motor_id].driver_state = state.actuator_ls_state[i].driver_state;
status_msg.actuator_states[motor_id].driver_voltage =
actuator.actuator_ls_state[i].driver_voltage;
status_msg.actuator_states[motor_id].driver_temperature =
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.custom_value =
state.light_state.front_light.custom_value;
@@ -186,21 +174,20 @@ namespace westonrobot
status_publisher_.publish(status_msg);
// 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
last_time_ = current_time_;
}
void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular)
{
void ScoutROSMessenger::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)
{
if (init_run) {
last_time_ = current_time_;
init_run = false;
return;
@@ -244,8 +231,7 @@ namespace westonrobot
}
void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular,
double dt)
{
double dt) {
// perform numerical integration to get an estimation of pose
linear_speed_ = linear;
angular_speed_ = angular;