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

Revert "Revert "Add turbo mode"" #40

Merged
merged 3 commits into from
Nov 7, 2024
Merged

Revert "Revert "Add turbo mode"" #40

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
fa27c51
Revert "Revert "Add turbo mode (#38)" (#39)"
TTaannaakkaa Nov 7, 2024
022781a
fix format
TTaannaakkaa Nov 7, 2024
62b510b
fix format
TTaannaakkaa Nov 7, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions include/motion_decision/motion_decision.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
#define RESET_COLOR "\033[0m"
#define RED "\033[31m"
#define GREEN "\033[32m"
#define CYAN "\033[36m"

#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
Expand All @@ -29,6 +30,7 @@ struct MotionDecisionParams
int hz;
int allowable_num_of_not_received;
float max_velocity;
float turbo_max_velocity;
float max_yawrate;
float dt;
float predict_time;
Expand Down Expand Up @@ -61,6 +63,7 @@ struct Flags
bool front_laser_updated = false;
bool rear_laser_updated = false;
bool move_trigger = false;
bool turbo_trigger = false;
};

struct Counters
Expand Down
2 changes: 2 additions & 0 deletions launch/motion_decision.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
<arg name="hz" default="20"/>
<arg name="allowable_num_of_not_received" default="3"/>
<arg name="max_velocity" default="1.0"/>
<arg name="turbo_max_velocity" default="1.66"/>
<arg name="max_yawrate" default="1.0"/>
<arg name="dt" default="0.1"/>
<arg name="predict_time" default="1.0"/>
Expand Down Expand Up @@ -41,6 +42,7 @@
<param name="hz" value="$(arg hz)"/>
<param name="allowable_num_of_not_received" value="$(arg allowable_num_of_not_received)"/>
<param name="max_velocity" value="$(arg max_velocity)"/>
<param name="turbo_max_velocity" value="$(arg turbo_max_velocity)"/>
<param name="max_yawrate" value="$(arg max_yawrate)"/>
<param name="dt" value="$(arg dt)"/>
<param name="predict_time" value="$(arg predict_time)"/>
Expand Down
52 changes: 45 additions & 7 deletions src/motion_decision.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@ void MotionDecision::load_params(void)
private_nh_.param<int>("hz", params_.hz, 20);
private_nh_.param<int>("allowable_num_of_not_received", params_.allowable_num_of_not_received, 3);
private_nh_.param<float>("max_velocity", params_.max_velocity, 1.0);
private_nh_.param<float>("turbo_max_velocity", params_.turbo_max_velocity, 1.66);
private_nh_.param<float>("max_yawrate", params_.max_yawrate, 1.0);
private_nh_.param<float>("dt", params_.dt, 0.1);
private_nh_.param<float>("predict_time", params_.predict_time, 1.0);
Expand Down Expand Up @@ -73,8 +74,17 @@ void MotionDecision::joy_callback(const sensor_msgs::JoyConstPtr &msg)
if (mode_.first == "move" && mode_.second == "manual")
{
flags_.move_trigger = msg->buttons[4];
cmd_vel_.linear.x = flags_.move_trigger ? msg->axes[1] * params_.max_velocity : 0.0;
cmd_vel_.angular.z = flags_.move_trigger ? msg->axes[0] * params_.max_yawrate : 0.0;
flags_.turbo_trigger = (flags_.move_trigger && msg->axes[2] == -1.0);
if (flags_.turbo_trigger)
{
cmd_vel_.linear.x = flags_.move_trigger ? msg->axes[1] * params_.turbo_max_velocity : 0.0;
cmd_vel_.angular.z = flags_.move_trigger ? msg->axes[0] * params_.max_yawrate : 0.0;
}
else
{
cmd_vel_.linear.x = flags_.move_trigger ? msg->axes[1] * params_.max_velocity : 0.0;
cmd_vel_.angular.z = flags_.move_trigger ? msg->axes[0] * params_.max_yawrate : 0.0;
}
}
else if (mode_.first == "move" && mode_.second == "auto" && !flags_.local_path_updated)
{
Expand Down Expand Up @@ -353,10 +363,36 @@ MotionDecision::sim_by_uniform_circluar_motion(const float &velocity, const floa

void MotionDecision::publish_cmd_vel(geometry_msgs::Twist cmd_vel)
{
cmd_vel.linear.x = 0.0 < cmd_vel.linear.x ? std::min(cmd_vel.linear.x, static_cast<double>(params_.max_velocity))
: std::max(cmd_vel.linear.x, static_cast<double>(-params_.max_velocity));
cmd_vel.angular.z = 0.0 < cmd_vel.angular.z ? std::min(cmd_vel.angular.z, static_cast<double>(params_.max_yawrate))
: std::max(cmd_vel.angular.z, -static_cast<double>(params_.max_yawrate));
if (flags_.turbo_trigger)
{
cmd_vel.linear.x =
0.0 < cmd_vel.linear.x
? std::min(cmd_vel.linear.x,
static_cast<double>(params_.turbo_max_velocity))
: std::max(cmd_vel.linear.x,
static_cast<double>(-params_.turbo_max_velocity));
cmd_vel.angular.z =
0.0 < cmd_vel.angular.z
? std::min(cmd_vel.angular.z,
static_cast<double>(params_.max_yawrate))
: std::max(cmd_vel.angular.z,
-static_cast<double>(params_.max_yawrate));
}
else
{
cmd_vel.linear.x =
0.0 < cmd_vel.linear.x
? std::min(cmd_vel.linear.x,
static_cast<double>(params_.max_velocity))
: std::max(cmd_vel.linear.x,
static_cast<double>(-params_.max_velocity));
cmd_vel.angular.z =
0.0 < cmd_vel.angular.z
? std::min(cmd_vel.angular.z,
static_cast<double>(params_.max_yawrate))
: std::max(cmd_vel.angular.z,
-static_cast<double>(params_.max_yawrate));
}

if (flags_.emergency_stop || mode_.first == "stop")
cmd_vel = geometry_msgs::Twist();
Expand Down Expand Up @@ -391,7 +427,9 @@ void MotionDecision::print_status(const geometry_msgs::Twist &cmd_vel)
{
if (mode_.first == "move" && mode_.second == "manual")
{
if (flags_.move_trigger)
if (flags_.turbo_trigger)
print_mode_status("TURBO", CYAN);
else if (flags_.move_trigger)
print_mode_status("UNLOCKED", GREEN);
else
print_mode_status("LOCKED", RED);
Expand Down
Loading