Skip to content

Add control_frequency_desired param #29

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

Merged
merged 1 commit into from
Feb 1, 2016
Merged

Add control_frequency_desired param #29

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
2 changes: 2 additions & 0 deletions diff_drive_controller/cfg/DiffDriveController.cfg
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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();
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

control_period_desired_ seems redundant, why not just do:
const double control_period = control_frequency_desired_ > 0.0 ? 1.0 /control_frequency_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
Open in desktop
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