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

Merge branch 'master' of https://gitlab.com/westonrobot/ros/scout_base

Browse Source
This commit is contained in:
Ruixiang Du
2020-10-12 17:39:26 +08:00
parent d1bfa60f70 a2bcb3c8c8
commit 7b7b9c1187
6 changed files with 232 additions and 206 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -10,12 +10,10 @@
#ifndef SCOUT_PARAMS_HPP
#define SCOUT_PARAMS_HPP
#include <cstdint>
namespace westonrobot
{
struct ScoutParams
{
struct ScoutParams
{
/* Scout Parameters */
static constexpr double max_steer_angle = 30.0; // in degree
@@ -29,7 +27,7 @@ struct ScoutParams
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 */

6
scout_base/launch/scout_base.launch
View File

@@ -11,16 +11,14 @@
-->
<arg name="port_name" default="can0" />
<arg name="simulated_robot" default="false" />
<arg name ="odom_topic_name" default = "odom"/>
<arg name="odom_topic_name" default="odom" />
<node name="scout_base_node" pkg="scout_base" type="scout_base_node" output="screen">
<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)" />
<param name="odom_topic_name" type="string" value="$(arg odom_topic_name)" />
</node>
</launch>

114
scout_base/src/scout_messenger.cpp
View File

@@ -13,16 +13,16 @@
#include "scout_msgs/ScoutStatus.h"
namespace westonrobot {
ScoutROSMessenger::ScoutROSMessenger(ros::NodeHandle *nh)
namespace westonrobot
{
ScoutROSMessenger::ScoutROSMessenger(ros::NodeHandle *nh)
: scout_(nullptr), nh_(nh) {}
ScoutROSMessenger::ScoutROSMessenger(ScoutBase *scout, ros::NodeHandle *nh)
ScoutROSMessenger::ScoutROSMessenger(ScoutBase *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_ =
@@ -33,66 +33,84 @@ void ScoutROSMessenger::SetupSubscription() {
"/cmd_vel", 5, &ScoutROSMessenger::TwistCmdCallback, this);
light_cmd_subscriber_ = nh_->subscribe<scout_msgs::ScoutLightCmd>(
"/scout_light_control", 5, &ScoutROSMessenger::LightCmdCallback, this);
}
}
void ScoutROSMessenger::TwistCmdCallback(
const geometry_msgs::Twist::ConstPtr &msg) {
if (!simulated_robot_) {
void ScoutROSMessenger::TwistCmdCallback(
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();
}
// ROS_INFO("cmd received:%f, %f", msg->linear.x, msg->angular.z);
}
}
void ScoutROSMessenger::GetCurrentMotionCmdForSim(double &linear,
double &angular) {
void ScoutROSMessenger::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(
const scout_msgs::ScoutLightCmd::ConstPtr &msg) {
if (!simulated_robot_) {
if (msg->enable_cmd_light_control) {
void ScoutROSMessenger::LightCmdCallback(
const scout_msgs::ScoutLightCmd::ConstPtr &msg)
{
if (!simulated_robot_)
{
if (msg->enable_cmd_light_control)
{
ScoutLightCmd cmd;
switch (msg->front_mode) {
case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: {
switch (msg->front_mode)
{
case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
{
cmd.front_mode = ScoutLightCmd::LightMode::CONST_OFF;
break;
}
case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: {
case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
{
cmd.front_mode = ScoutLightCmd::LightMode::CONST_ON;
break;
}
case scout_msgs::ScoutLightCmd::LIGHT_BREATH: {
case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
{
cmd.front_mode = ScoutLightCmd::LightMode::BREATH;
break;
}
case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: {
case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
{
cmd.front_mode = ScoutLightCmd::LightMode::CUSTOM;
cmd.front_custom_value = msg->front_custom_value;
break;
}
}
switch (msg->rear_mode) {
case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: {
switch (msg->rear_mode)
{
case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
{
cmd.rear_mode = ScoutLightCmd::LightMode::CONST_OFF;
break;
}
case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: {
case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
{
cmd.rear_mode = ScoutLightCmd::LightMode::CONST_ON;
break;
}
case scout_msgs::ScoutLightCmd::LIGHT_BREATH: {
case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
{
cmd.rear_mode = ScoutLightCmd::LightMode::BREATH;
break;
}
case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: {
case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
{
cmd.rear_mode = ScoutLightCmd::LightMode::CUSTOM;
cmd.rear_custom_value = msg->rear_custom_value;
break;
@@ -100,21 +118,26 @@ void ScoutROSMessenger::LightCmdCallback(
}
scout_->SetLightCommand(cmd);
} else {
}
else
{
scout_->DisableLightCmdControl();
}
} else {
}
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;
@@ -135,7 +158,8 @@ void ScoutROSMessenger::PublishStateToROS() {
status_msg.fault_code = state.fault_code;
status_msg.battery_voltage = state.battery_voltage;
for (int i = 0; i < 4; ++i) {
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;
@@ -155,21 +179,22 @@ void ScoutROSMessenger::PublishStateToROS() {
// 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;
}
// publish scout state message
scout_msgs::ScoutStatus status_msg;
@@ -205,10 +230,11 @@ void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) {
// record time for next integration
last_time_ = current_time_;
}
}
void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular,
double dt) {
void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular,
double dt)
{
// perform numerical integration to get an estimation of pose
linear_speed_ = linear;
angular_speed_ = angular;
@@ -252,5 +278,5 @@ void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular,
odom_msg.twist.twist.angular.z = angular_speed_;
odom_publisher_.publish(odom_msg);
}
}
} // namespace westonrobot

12
scout_bringup/launch/scout_minimal.launch
View File

@@ -2,17 +2,17 @@
<arg name="port_name" value="can0" />
<arg name="simulated_robot" value="false" />
<arg name = "model_xacro" default = "$(find scout_description)/urdf/scout_v2.xacro" />
<arg name ="odom_topic_name" default = "odomsdas"/>
<arg name="model_xacro" default="$(find scout_description)/urdf/scout_v2.xacro" />
<arg name="odom_topic_name" default="odom" />
<include file="$(find scout_base)/launch/scout_base.launch" >
<include file="$(find scout_base)/launch/scout_base.launch">
<arg name="port_name" default="$(arg port_name)" />
<arg name="simulated_robot" default="$(arg simulated_robot)" />
<arg name="odom_topic_name" default="$(arg odom_topic_name)" />
</include>
<include file="$(find scout_description)/launch/description.launch" >
<arg name = "model_xacro" default = "$(arg model_xacro)" />
<include file="$(find scout_description)/launch/description.launch">
<arg name="model_xacro" default="$(arg model_xacro)" />
</include>
</launch>

11
scout_bringup/launch/scout_minimal_uart.launch
View File

@@ -2,17 +2,18 @@
<arg name="port_name" value="/dev/ttyUSB0" />
<arg name="simulated_robot" value="false" />
<arg name = "model_xacro" default = "$(find scout_description)/urdf/scout_v2.xacro" />
<arg name ="odom_topic_name" default = "odomsdas"/>
<arg name="model_xacro" default="$(find scout_description)/urdf/scout_v2.xacro" />
<arg name="odom_topic_name" default="odom" />
<include file="$(find scout_base)/launch/scout_base.launch" >
<include file="$(find scout_base)/launch/scout_base.launch">
<arg name="port_name" default="$(arg port_name)" />
<arg name="simulated_robot" default="$(arg simulated_robot)" />
<arg name="odom_topic_name" default="$(arg odom_topic_name)" />
</include>
<include file="$(find scout_description)/launch/description.launch" >
<arg name = "model_xacro" default = "$(arg model_xacro)" />
<include file="$(find scout_description)/launch/description.launch">
<arg name="model_xacro" default="$(arg model_xacro)" />
</include>
</launch>

3
scout_bringup/launch/scout_teleop_keyboard.launch
View File

@@ -1,4 +1,7 @@
<launch>
<arg name="repeat_rate" value="50.0" />
<node name="teleop_keybord" pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" output="screen">
<param name="repeat_rate" value="$(arg repeat_rate)" />
</node>
</launch>