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

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 #ifndef SCOUT_PARAMS_HPP
#define SCOUT_PARAMS_HPP #define SCOUT_PARAMS_HPP
#include <cstdint>
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
@@ -29,7 +27,7 @@ struct ScoutParams
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 */

6
scout_base/launch/scout_base.launch
View File

@@ -11,16 +11,14 @@
--> -->
<arg name="port_name" default="can0" /> <arg name="port_name" default="can0" />
<arg name="simulated_robot" default="false" /> <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"> <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="port_name" type="string" value="$(arg port_name)" />
<param name="simulated_robot" type="bool" value="$(arg simulated_robot)" /> <param name="simulated_robot" type="bool" value="$(arg simulated_robot)" />
<param name="odom_frame" type="string" value="odom" /> <param name="odom_frame" type="string" value="odom" />
<param name="base_frame" type="string" value="base_link" /> <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> </node>
</launch> </launch>

114
scout_base/src/scout_messenger.cpp
View File

@@ -13,16 +13,16 @@
#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(ScoutBase *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,66 +33,84 @@ void ScoutROSMessenger::SetupSubscription() {
"/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 (msg->enable_cmd_light_control) { if (!simulated_robot_)
{
if (msg->enable_cmd_light_control)
{
ScoutLightCmd cmd; 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 = ScoutLightCmd::LightMode::CONST_OFF; cmd.front_mode = ScoutLightCmd::LightMode::CONST_OFF;
break; break;
} }
case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: { case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
{
cmd.front_mode = ScoutLightCmd::LightMode::CONST_ON; cmd.front_mode = ScoutLightCmd::LightMode::CONST_ON;
break; break;
} }
case scout_msgs::ScoutLightCmd::LIGHT_BREATH: { case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
{
cmd.front_mode = ScoutLightCmd::LightMode::BREATH; cmd.front_mode = ScoutLightCmd::LightMode::BREATH;
break; break;
} }
case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: { case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
{
cmd.front_mode = ScoutLightCmd::LightMode::CUSTOM; cmd.front_mode = ScoutLightCmd::LightMode::CUSTOM;
cmd.front_custom_value = msg->front_custom_value; cmd.front_custom_value = msg->front_custom_value;
break; break;
} }
} }
switch (msg->rear_mode) { switch (msg->rear_mode)
case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF: { {
case scout_msgs::ScoutLightCmd::LIGHT_CONST_OFF:
{
cmd.rear_mode = ScoutLightCmd::LightMode::CONST_OFF; cmd.rear_mode = ScoutLightCmd::LightMode::CONST_OFF;
break; break;
} }
case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON: { case scout_msgs::ScoutLightCmd::LIGHT_CONST_ON:
{
cmd.rear_mode = ScoutLightCmd::LightMode::CONST_ON; cmd.rear_mode = ScoutLightCmd::LightMode::CONST_ON;
break; break;
} }
case scout_msgs::ScoutLightCmd::LIGHT_BREATH: { case scout_msgs::ScoutLightCmd::LIGHT_BREATH:
{
cmd.rear_mode = ScoutLightCmd::LightMode::BREATH; cmd.rear_mode = ScoutLightCmd::LightMode::BREATH;
break; break;
} }
case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM: { case scout_msgs::ScoutLightCmd::LIGHT_CUSTOM:
{
cmd.rear_mode = ScoutLightCmd::LightMode::CUSTOM; cmd.rear_mode = ScoutLightCmd::LightMode::CUSTOM;
cmd.rear_custom_value = msg->rear_custom_value; cmd.rear_custom_value = msg->rear_custom_value;
break; break;
@@ -100,21 +118,26 @@ void ScoutROSMessenger::LightCmdCallback(
} }
scout_->SetLightCommand(cmd); scout_->SetLightCommand(cmd);
} else { }
else
{
scout_->DisableLightCmdControl(); scout_->DisableLightCmdControl();
} }
} else { }
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;
@@ -135,7 +158,8 @@ void ScoutROSMessenger::PublishStateToROS() {
status_msg.fault_code = state.fault_code; status_msg.fault_code = state.fault_code;
status_msg.battery_voltage = state.battery_voltage; 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].current = state.motor_states[i].current;
status_msg.motor_states[i].rpm = state.motor_states[i].rpm; status_msg.motor_states[i].rpm = state.motor_states[i].rpm;
status_msg.motor_states[i].temperature = state.motor_states[i].temperature; status_msg.motor_states[i].temperature = state.motor_states[i].temperature;
@@ -155,21 +179,22 @@ void ScoutROSMessenger::PublishStateToROS() {
// 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;
} }
// publish scout state message // publish scout state message
scout_msgs::ScoutStatus status_msg; scout_msgs::ScoutStatus status_msg;
@@ -205,10 +230,11 @@ void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) {
// 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;
@@ -252,5 +278,5 @@ void ScoutROSMessenger::PublishOdometryToROS(double linear, double angular,
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

12
scout_bringup/launch/scout_minimal.launch
View File

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

11
scout_bringup/launch/scout_minimal_uart.launch
View File

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

3
scout_bringup/launch/scout_teleop_keyboard.launch
View File

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