Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add cmd vel expire option #55

Merged
merged 2 commits into from Apr 30, 2019
Merged

Add cmd vel expire option #55

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b8aa84e
Add parameter to set expire duration of cmd_vel
at-wat Apr 29, 2019
048705e
Update document
at-wat Apr 29, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
5 changes: 5 additions & 0 deletions doc/ypspur_ros.md
View file
Open in desktop
Expand Up @@ -60,6 +60,11 @@ This topic name is configurable through `ad?_name` parameter.

Path to your vehicle parameter file for yp-spur.

### ~/cmd_vel_expire [double: -1.0]

Expire duration of cmd_vel. After this duration since receiving cmd_vel, velocity and angular velocity is set to zero.
Negative or zero value disables this feature.

### ~/ad?_enable [bool: false]

### ~/ad?_name [string: "ad" + i]
Expand Down
36 changes: 29 additions & 7 deletions src/ypspur_ros.cpp
View file
Open in desktop
Expand Up @@ -159,7 +159,9 @@ class YpspurRosNode
int device_error_state_prev_;
ros::Time device_error_state_time_;

geometry_msgs::Twist cmd_vel_;
geometry_msgs::Twist::ConstPtr cmd_vel_;
ros::Time cmd_vel_time_;
ros::Duration cmd_vel_expire_;

int control_mode_;

Expand All @@ -180,7 +182,8 @@ class YpspurRosNode
}
void cbCmdVel(const geometry_msgs::Twist::ConstPtr &msg)
{
cmd_vel_ = *msg;
cmd_vel_ = msg;
cmd_vel_time_ = ros::Time::now();
if (control_mode_ == ypspur_ros::ControlMode::VELOCITY)
{
YP::YPSpur_vel(msg->linear.x, msg->angular.z);
Expand Down Expand Up @@ -464,6 +467,11 @@ class YpspurRosNode
pnh_.param("ypspur_bin", ypspur_bin_, std::string("ypspur-coordinator"));
pnh_.param("param_file", param_file_, std::string(""));
pnh_.param("tf_time_offset", tf_time_offset_, 0.0);

double cmd_vel_expire_s;
pnh_.param("cmd_vel_expire", cmd_vel_expire_s, -1.0);
cmd_vel_expire_ = ros::Duration(cmd_vel_expire_s);

std::string ad_mask("");
ads_.resize(ad_num_);
for (int i = 0; i < ad_num_; i++)
Expand Down Expand Up @@ -829,12 +837,23 @@ class YpspurRosNode
ros::Rate loop(params_["hz"]);
while (!g_shutdown)
{
auto now = ros::Time::now();
float dt = 1.0 / params_["hz"];
const auto now = ros::Time::now();
const float dt = 1.0 / params_["hz"];

if (cmd_vel_ && cmd_vel_expire_ > ros::Duration(0))
{
if (cmd_vel_time_ + cmd_vel_expire_ < now)
{
// cmd_vel is too old and expired
cmd_vel_ = nullptr;
if (control_mode_ == ypspur_ros::ControlMode::VELOCITY)
YP::YPSpur_vel(0.0, 0.0);
}
}

if (mode_ == DIFF)
{
double x, y, yaw, v, w;
double x, y, yaw, v(0), w(0);
double t;

if (!simulate_control_)
Expand All @@ -847,8 +866,11 @@ class YpspurRosNode
else
{
t = ros::Time::now().toSec();
v = cmd_vel_.linear.x;
w = cmd_vel_.angular.z;
if (cmd_vel_)
{
v = cmd_vel_->linear.x;
w = cmd_vel_->angular.z;
}
yaw = tf::getYaw(odom.pose.pose.orientation) + dt * w;
x = odom.pose.pose.position.x + dt * v * cosf(yaw);
y = odom.pose.pose.position.y + dt * v * sinf(yaw);
Expand Down