Skip to content

Commit

Permalink
Merge pull request #29 from clearpathrobotics/add_control_frequency_d…
Browse files Browse the repository at this point in the history 
…esired_param

Add control_frequency_desired param
  • Loading branch information
Enrique Fernández Perdomo committed Feb 1, 2016
2 parents a990fd2 + 5f27f31 commit 1ba314f
Show file tree
Hide file tree
Showing 5 changed files with 75 additions and 22 deletions.
2 changes: 2 additions & 0 deletions diff_drive_controller/cfg/DiffDriveController.cfg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,4 +19,6 @@ gen.add("k_r", double_t, 0, "Covariance model k_r multiplier.", 1.0, 0.0, 10.0)
gen.add("publish_cmd_vel_limited", bool_t, 0, "Publish limited velocity command.", False)
gen.add("publish_state", bool_t, 0, "Publish joint trajectory controller state.", False)

gen.add("control_frequency_desired", double_t, 0, "Desired/Expected control frequency [Hz].", 0.0, 0.0, 500.0)

exit(gen.generate(PACKAGE, "diff_drive_controller", "DiffDriveController"))
12 changes: 12 additions & 0 deletions diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -200,6 +200,8 @@ namespace diff_drive_controller
bool publish_state;
bool publish_cmd_vel_limited;

double control_frequency_desired;

DynamicParams()
: pose_from_joint_position(true)
, twist_from_joint_position(false)
Expand All @@ -210,6 +212,7 @@ namespace diff_drive_controller
, k_r(1.0)
, publish_state(false)
, publish_cmd_vel_limited(false)
, control_frequency_desired(0.0)
{}
};
realtime_tools::RealtimeBuffer<DynamicParams> dynamic_params_;
Expand Down Expand Up @@ -262,6 +265,15 @@ namespace diff_drive_controller
/// state_pub_->getNumSubscribers() > 0
bool publish_state_;

/// Desired control frequency [Hz] (and corresponding period [s]):
/// This can be used when the actual/real control frequency/period has too
/// much jitter.
/// If !(control_frequency_desired_ > 0.0), the actual/real
/// frequency/period provided on the update hook (method) is used, which is
/// the default and preferred behaviour in general.
double control_frequency_desired_;
double control_period_desired_;

private:
/**
* \brief Brakes the wheels, i.e. sets the velocity to 0
Expand Down
5 changes: 5 additions & 0 deletions diff_drive_controller/msg/DiffDriveControllerState.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,3 +26,8 @@ trajectory_msgs/JointTrajectoryPoint error_side_average
# joints, but estimated:
trajectory_msgs/JointTrajectoryPoint actual_estimated_side_average
trajectory_msgs/JointTrajectoryPoint error_estimated_side_average

# Control period:
float64 control_period_desired
float64 control_period_actual
float64 control_period_error
76 changes: 54 additions & 22 deletions diff_drive_controller/src/diff_drive_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -140,6 +140,8 @@ namespace diff_drive_controller
, wheel_joints_size_(0)
, publish_cmd_vel_limited_(false)
, publish_state_(false)
, control_frequency_desired_(0.0)
, control_period_desired_(0.0)
{
}

Expand Down Expand Up @@ -262,6 +264,19 @@ namespace diff_drive_controller
"Publishing the joint trajectory controller state is "
<< (publish_state_?"enabled":"disabled"));

controller_nh.param("control_frequency_desired", control_frequency_desired_, control_frequency_desired_);
if (control_frequency_desired_ > 0.0)
{
control_period_desired_ = 1.0 / control_frequency_desired_;

ROS_INFO_STREAM_NAMED(name_, "Using desired/expected "
<< control_frequency_desired_ << "Hz control frequency.");
}
else
{
ROS_INFO_STREAM_NAMED(name_, "Using real control frequency.");
}

// Velocity and acceleration limits:
controller_nh.param("linear/x/has_velocity_limits" , limiter_lin_.has_velocity_limits , limiter_lin_.has_velocity_limits );
controller_nh.param("linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits);
Expand Down Expand Up @@ -330,6 +345,8 @@ namespace diff_drive_controller
dynamic_params_struct_.publish_state = publish_state_;
dynamic_params_struct_.publish_cmd_vel_limited = publish_cmd_vel_limited_;

dynamic_params_struct_.control_frequency_desired = control_frequency_desired_;

dynamic_params_.writeFromNonRT(dynamic_params_struct_);

setOdomPubFields(root_nh, controller_nh);
Expand Down Expand Up @@ -478,6 +495,9 @@ namespace diff_drive_controller
publish_state_ = dynamic_params.publish_state;
publish_cmd_vel_limited_ = dynamic_params.publish_cmd_vel_limited;

control_frequency_desired_ = dynamic_params.control_frequency_desired;
control_period_desired_ = control_frequency_desired_ > 0.0 ? 1.0 / control_frequency_desired_ : 0.0;

// Apply multipliers:
const double ws = wheel_separation_multiplier_ * wheel_separation_;
const double wrl = left_wheel_radius_multiplier_ * wheel_radius_;
Expand Down Expand Up @@ -529,18 +549,22 @@ namespace diff_drive_controller
}
}

// Compute/Set control period and frequency (desired/expected or real):
const double control_period = control_period_desired_ > 0.0 ? control_period_desired_ : period.toSec();
const double control_frequency = control_frequency_desired_ > 0.0 ? control_frequency_desired_ : 1.0 / control_period;

// Compute wheel joints positions estimated from the velocities:
for (size_t i = 0; i < wheel_joints_size_; ++i)
{
left_positions_estimated_[i] += left_velocities_[i] * period.toSec();
right_positions_estimated_[i] += right_velocities_[i] * period.toSec();
left_positions_estimated_[i] += left_velocities_[i] * control_period;
right_positions_estimated_[i] += right_velocities_[i] * control_period;
}

// Compute wheel joints velocities estimated from the positions:
for (size_t i = 0; i < wheel_joints_size_; ++i)
{
left_velocities_estimated_[i] = (left_positions_[i] - left_positions_previous_[i]) / period.toSec();
right_velocities_estimated_[i] = (right_positions_[i] - right_positions_previous_[i]) / period.toSec();
left_velocities_estimated_[i] = (left_positions_[i] - left_positions_previous_[i]) * control_frequency;
right_velocities_estimated_[i] = (right_positions_[i] - right_positions_previous_[i]) * control_frequency;
}

// Compute wheel joints poisitions average per side:
Expand Down Expand Up @@ -578,7 +602,7 @@ namespace diff_drive_controller
}

// Publish odometry message:
const ros::Duration half_period(0.5 * period.toSec());
const ros::Duration half_period(0.5 * control_period);
if (last_odom_publish_time_ + publish_period_ < time + half_period &&
odom_pub_->trylock())
{
Expand Down Expand Up @@ -635,11 +659,9 @@ namespace diff_drive_controller
}

// Limit velocities and accelerations:
const double cmd_dt(period.toSec());

// @todo add an option to limit the velocity considering the actual velocity
limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, cmd_dt);
limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, cmd_dt);
limiter_lin_.limit(curr_cmd.lin, last0_cmd_.lin, last1_cmd_.lin, control_period);
limiter_ang_.limit(curr_cmd.ang, last0_cmd_.ang, last1_cmd_.ang, control_period);

last1_cmd_ = last0_cmd_;
last0_cmd_ = curr_cmd;
Expand Down Expand Up @@ -677,21 +699,21 @@ namespace diff_drive_controller
for (size_t i = 0; i < wheel_joints_size_; ++i)
{
// Desired state:
state_pub_->msg_.desired.accelerations[i] = (left_velocity_command - left_velocity_command_previous_) / period.toSec();
state_pub_->msg_.desired.accelerations[i] = (left_velocity_command - left_velocity_command_previous_) * control_frequency;
state_pub_->msg_.desired.velocities[i] = left_velocity_command;
state_pub_->msg_.desired.positions[i] += left_velocity_command * period.toSec();
state_pub_->msg_.desired.positions[i] += left_velocity_command * control_period;
state_pub_->msg_.desired.effort[i] = std::numeric_limits<double>::quiet_NaN();

// Actual state:
const double left_acceleration = (left_velocities_[i] - left_velocities_previous_[i]) / period.toSec();
const double left_acceleration = (left_velocities_[i] - left_velocities_previous_[i]) * control_frequency;

state_pub_->msg_.actual.accelerations[i] = left_acceleration;
state_pub_->msg_.actual.velocities[i] = left_velocities_[i];
state_pub_->msg_.actual.positions[i] = left_positions_[i];
state_pub_->msg_.actual.effort[i] = left_wheel_joints_[i].getEffort();

// Actual estimated state:
const double left_acceleration_estimated = (left_velocities_estimated_[i] - left_velocities_estimated_previous_[i]) / period.toSec();
const double left_acceleration_estimated = (left_velocities_estimated_[i] - left_velocities_estimated_previous_[i]) * control_frequency;

state_pub_->msg_.actual_estimated.accelerations[i] = left_acceleration_estimated;
state_pub_->msg_.actual_estimated.velocities[i] = left_velocities_estimated_[i];
Expand All @@ -703,21 +725,21 @@ namespace diff_drive_controller
for (size_t i = 0, j = wheel_joints_size_; i < wheel_joints_size_; ++i, ++j)
{
// Desired state:
state_pub_->msg_.desired.accelerations[j] = (right_velocity_command - right_velocity_command_previous_) / period.toSec();
state_pub_->msg_.desired.accelerations[j] = (right_velocity_command - right_velocity_command_previous_) * control_frequency;
state_pub_->msg_.desired.velocities[j] = right_velocity_command;
state_pub_->msg_.desired.positions[j] += right_velocity_command * period.toSec();
state_pub_->msg_.desired.positions[j] += right_velocity_command * control_period;
state_pub_->msg_.desired.effort[j] = std::numeric_limits<double>::quiet_NaN();

// Actual state:
const double right_acceleration = (right_velocities_[i] - right_velocities_previous_[i]) / period.toSec();
const double right_acceleration = (right_velocities_[i] - right_velocities_previous_[i]) * control_frequency;

state_pub_->msg_.actual.accelerations[j] = right_acceleration;
state_pub_->msg_.actual.velocities[j] = right_velocities_[i];
state_pub_->msg_.actual.positions[j] = right_positions_[i];
state_pub_->msg_.actual.effort[j] = right_wheel_joints_[i].getEffort();

// Actual estimated state:
const double right_acceleration_estimated = (right_velocities_estimated_[i] - right_velocities_estimated_previous_[i]) / period.toSec();
const double right_acceleration_estimated = (right_velocities_estimated_[i] - right_velocities_estimated_previous_[i]) * control_frequency;

state_pub_->msg_.actual_estimated.accelerations[j] = right_acceleration_estimated;
state_pub_->msg_.actual_estimated.velocities[j] = right_velocities_estimated_[i];
Expand All @@ -727,14 +749,14 @@ namespace diff_drive_controller

// Set left wheel joints actual (and actual estimated) side average
// state:
const double left_acceleration_average = (left_velocity_average - left_velocity_average_previous_) / period.toSec();
const double left_acceleration_average = (left_velocity_average - left_velocity_average_previous_) * control_frequency;

state_pub_->msg_.actual_side_average.accelerations[0] = left_acceleration_average;
state_pub_->msg_.actual_side_average.velocities[0] = left_velocity_average;
state_pub_->msg_.actual_side_average.positions[0] = left_position_average;
state_pub_->msg_.actual_side_average.effort[0] = state_pub_->msg_.actual.effort[0];

const double left_acceleration_estimated_average = (left_velocity_estimated_average - left_velocity_estimated_average_previous_) / period.toSec();
const double left_acceleration_estimated_average = (left_velocity_estimated_average - left_velocity_estimated_average_previous_) * control_frequency;

state_pub_->msg_.actual_estimated_side_average.accelerations[0] = left_acceleration_estimated_average;
state_pub_->msg_.actual_estimated_side_average.velocities[0] = left_velocity_estimated_average;
Expand All @@ -743,14 +765,14 @@ namespace diff_drive_controller

// Set right wheel joints actual (and actual estimated) side average
// state:
const double right_acceleration_average = (right_velocity_average - right_velocity_average_previous_) / period.toSec();
const double right_acceleration_average = (right_velocity_average - right_velocity_average_previous_) * control_frequency;

state_pub_->msg_.actual_side_average.accelerations[1] = right_acceleration_average;
state_pub_->msg_.actual_side_average.velocities[1] = right_velocity_average;
state_pub_->msg_.actual_side_average.positions[1] = right_position_average;
state_pub_->msg_.actual_side_average.effort[1] = state_pub_->msg_.actual.effort[wheel_joints_size_];

const double right_acceleration_estimated_average = (right_velocity_estimated_average - right_velocity_estimated_average_previous_) / period.toSec();
const double right_acceleration_estimated_average = (right_velocity_estimated_average - right_velocity_estimated_average_previous_) * control_frequency;

state_pub_->msg_.actual_estimated_side_average.accelerations[1] = right_acceleration_estimated_average;
state_pub_->msg_.actual_estimated_side_average.velocities[1] = right_velocity_estimated_average;
Expand Down Expand Up @@ -797,7 +819,7 @@ namespace diff_drive_controller
}

state_pub_->msg_.desired.time_from_start = ros::Duration(dt);
state_pub_->msg_.actual.time_from_start = period;
state_pub_->msg_.actual.time_from_start = ros::Duration(control_period);
state_pub_->msg_.error.time_from_start = state_pub_->msg_.actual.time_from_start;

state_pub_->msg_.actual_estimated.time_from_start = state_pub_->msg_.actual.time_from_start;
Expand All @@ -809,6 +831,10 @@ namespace diff_drive_controller
state_pub_->msg_.actual_estimated_side_average.time_from_start = state_pub_->msg_.actual.time_from_start;
state_pub_->msg_.error_estimated_side_average.time_from_start = state_pub_->msg_.actual_estimated_side_average.time_from_start;

state_pub_->msg_.control_period_desired = control_period_desired_;
state_pub_->msg_.control_period_actual = period.toSec();
state_pub_->msg_.control_period_error = state_pub_->msg_.control_period_desired - state_pub_->msg_.control_period_actual;

state_pub_->unlockAndPublish();
}

Expand Down Expand Up @@ -904,6 +930,8 @@ namespace diff_drive_controller
dynamic_params_struct_.publish_state = config.publish_state;
dynamic_params_struct_.publish_cmd_vel_limited = config.publish_cmd_vel_limited;

dynamic_params_struct_.control_frequency_desired = config.control_frequency_desired;

dynamic_params_.writeFromNonRT(dynamic_params_struct_);

ROS_DEBUG_STREAM_NAMED(name_,
Expand All @@ -923,6 +951,10 @@ namespace diff_drive_controller
"Reconfigured Debug Publishers params. "
<< "state: " << ( dynamic_params_struct_.publish_state ? "ON" : "OFF" ) << ", "
<< "cmd_vel_limited: " << ( dynamic_params_struct_.publish_cmd_vel_limited ? "ON" : "OFF" ));

ROS_DEBUG_STREAM_NAMED(name_,
"Reconfigured Control params. "
<< "desired/expected control frequency: " << dynamic_params_struct_.control_frequency_desired << "Hz");
}

bool DiffDriveController::getWheelNames(ros::NodeHandle& controller_nh,
Expand Down
2 changes: 2 additions & 0 deletions diff_drive_controller/src/odometry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,6 +131,8 @@ namespace diff_drive_controller
//
// this method would be use here and to 'Compute wheels velocities' in
// diff_drive_controller; here is displacement, there is velocity
//double v_l, v_r;
//inverseKinematics(linear, angular, v_l, v_r);
const double v_l = (linear - angular * wheel_separation_ / 2.0) / left_wheel_radius_;
const double v_r = (linear + angular * wheel_separation_ / 2.0) / right_wheel_radius_;

Expand Down

0 comments on commit 1ba314f

Please sign in to comment.