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 'PD_dev' into 'master'

Browse Source 
Pd dev

See merge request westonrobot/ros/scout_base!5
This commit is contained in:
Ruixiang Du
2020-09-18 09:58:32 +00:00
parent 31ff7d2878 910a1e08f8
commit a2bcb3c8c8
6 changed files with 232 additions and 206 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -10,8 +10,6 @@
#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

2
scout_base/launch/scout_base.launch
View File

@@ -11,13 +11,11 @@
--> -->
<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)" />

88
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_ =
@@ -36,10 +36,14 @@ void ScoutROSMessenger::SetupSubscription() {
} }
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();
} }
@@ -47,52 +51,66 @@ void ScoutROSMessenger::TwistCmdCallback(
} }
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;
@@ -157,19 +181,20 @@ void ScoutROSMessenger::PublishStateToROS() {
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;
@@ -208,7 +233,8 @@ void ScoutROSMessenger::PublishSimStateToROS(double linear, double angular) {
} }
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;

4
scout_bringup/launch/scout_minimal.launch
View File

@@ -3,16 +3,16 @@
<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>

3
scout_bringup/launch/scout_minimal_uart.launch
View File

@@ -3,7 +3,7 @@
<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)" />
@@ -15,4 +15,5 @@
<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>