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

Allow computing pose/twist from position/velocity #18

Merged
merged 1 commit into from
Feb 1, 2016
Merged

Allow computing pose/twist from position/velocity #18

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
5 changes: 4 additions & 1 deletion diff_drive_controller/cfg/DiffDriveController.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,14 +6,17 @@ from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, b

gen = ParameterGenerator()

gen.add("pose_from_joint_position", bool_t, 0, "Compute odometry pose from wheel joint position.", True)
gen.add("twist_from_joint_position", bool_t, 0, "Compute odometry twist from wheel joint position.", False)

gen.add("wheel_separation_multiplier", double_t, 0, "Wheel separation multiplier.", 1.0, 0.5, 1.5)
gen.add("left_wheel_radius_multiplier", double_t, 0, "Left wheel radius multiplier.", 1.0, 0.5, 1.5)
gen.add("right_wheel_radius_multiplier", double_t, 0, "Right wheel radius multiplier.", 1.0, 0.5, 1.5)

gen.add("k_l", double_t, 0, "Covariance model k_l multiplier.", 1.0, 0.0, 10.0)
gen.add("k_r", double_t, 0, "Covariance model k_r multiplier.", 1.0, 0.0, 10.0)

gen.add("publish_state", bool_t, 0, "Publish joint trajectory controller state.", False)
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)

exit(gen.generate(PACKAGE, "diff_drive_controller", "DiffDriveController"))
14 changes: 12 additions & 2 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 @@ -117,7 +117,12 @@ namespace diff_drive_controller
ros::Duration publish_period_;
ros::Time last_odom_publish_time_;
bool open_loop_;
bool position_feedback_;

bool pose_from_joint_position_;
bool twist_from_joint_position_;

bool use_position_;
bool use_velocity_;

/// Hardware handles:
std::vector<hardware_interface::JointHandle> left_wheel_joints_;
Expand Down Expand Up @@ -181,6 +186,9 @@ namespace diff_drive_controller

struct DynamicParams
{
bool pose_from_joint_position;
bool twist_from_joint_position;

double wheel_separation_multiplier;
double left_wheel_radius_multiplier;
double right_wheel_radius_multiplier;
Expand All @@ -192,7 +200,9 @@ namespace diff_drive_controller
bool publish_cmd_vel_limited;

DynamicParams()
: wheel_separation_multiplier(1.0)
: pose_from_joint_position(true)
, twist_from_joint_position(false)
, wheel_separation_multiplier(1.0)
, left_wheel_radius_multiplier(1.0)
, right_wheel_radius_multiplier(1.0)
, k_l(1.0)
Expand Down
30 changes: 13 additions & 17 deletions diff_drive_controller/include/diff_drive_controller/odometry.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -115,17 +115,10 @@ namespace diff_drive_controller
* \param time [in] Current time
* \return true if the odometry is actually updated
*/
bool updateCloseLoop(double left_pos, double right_pos, const ros::Time &time);

/**
* \brief Updates the odometry class with latest wheels velocity, i.e. in
* close loop
* \param left_vel [in] Left wheel velocity [rad/s]
* \param right_vel [in] Right wheel velocity [rad/s]
* \param time [in] Current time
* \return true if the odometry is actually updated
*/
bool updateCloseLoopFromVelocity(double left_pos, double right_pos, const ros::Time &time);
bool updateCloseLoop(
const double left_position, const double right_position,
const double left_velocity, const double right_velocity,
const ros::Time &time);

/**
* \brief Updates the odometry class with latest velocity command, i.e. in
Expand Down Expand Up @@ -267,12 +260,15 @@ namespace diff_drive_controller
/**
* \brief Updates the odometry class with latest velocity command and wheel
* velocities
* \param v_l [in] Left wheel velocity displacement [rad]
* \param v_r [in] Right wheel velocity displacement [rad]
* \param time [in] Current time
* \param[in] dp_l Left wheel position increment [rad]
* \param[in] dp_r Right wheel position increment [rad]
* \param[in] v_l Left wheel velocity [rad/s]
* \param[in] v_r Right wheel velocity [rad/s]
* \param[in] time Current time
* \return true if the odometry is actually updated
*/
bool update(double v_l, double v_r, const ros::Time& time);
bool update(const double dp_l, const double dp_r,
const double v_l, const double v_r, const ros::Time& time);

/**
* \brief Update the odometry twist with the previous and current odometry
Expand Down Expand Up @@ -332,8 +328,8 @@ namespace diff_drive_controller
double k_r_;

/// Previous wheel position/state [rad]:
double left_wheel_old_pos_;
double right_wheel_old_pos_;
double left_position_previous_;
double right_position_previous_;

/// Rolling mean accumulators for the linar and angular velocities:
size_t velocity_rolling_window_size_;
Expand Down
116 changes: 75 additions & 41 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 @@ -123,7 +123,8 @@ namespace diff_drive_controller

DiffDriveController::DiffDriveController()
: open_loop_(false)
, position_feedback_(true)
, pose_from_joint_position_(true)
, twist_from_joint_position_(false)
, command_struct_()
, dynamic_params_struct_()
, wheel_separation_(0.0)
Expand Down Expand Up @@ -184,10 +185,18 @@ namespace diff_drive_controller
ROS_INFO_STREAM_NAMED(name_, "Odometry will be computed in "
<< (open_loop_ ? "open" : "close") << " loop.");

controller_nh.param("position_feedback", position_feedback_, position_feedback_);
ROS_DEBUG_STREAM_COND_NAMED(!open_loop_, name_,
"Odometry will be computed using the wheel " <<
(position_feedback_ ? "postion" : "velocity") << " feedback.");
controller_nh.param("pose_from_joint_position", pose_from_joint_position_, pose_from_joint_position_);
ROS_INFO_STREAM_COND_NAMED(!open_loop_, name_,
"Odometry pose will be computed using the wheel joint " <<
(pose_from_joint_position_ ? "position" : "velocity") << " feedback.");

controller_nh.param("twist_from_joint_position", twist_from_joint_position_, twist_from_joint_position_);
ROS_INFO_STREAM_COND_NAMED(!open_loop_, name_,
"Odometry twist will be computed using the wheel joint " <<
(twist_from_joint_position_ ? "position" : "velocity") << " feedback.");

use_position_ = pose_from_joint_position_ || twist_from_joint_position_;
use_velocity_ = !pose_from_joint_position_ || !twist_from_joint_position_;

controller_nh.param("wheel_separation_multiplier",
wheel_separation_multiplier_, wheel_separation_multiplier_);
Expand Down Expand Up @@ -303,6 +312,9 @@ namespace diff_drive_controller
"Measurement Covariance Model params : k_l " << k_l_
<< ", k_r " << k_r_);

dynamic_params_struct_.pose_from_joint_position = pose_from_joint_position_;
dynamic_params_struct_.twist_from_joint_position = twist_from_joint_position_;

dynamic_params_struct_.wheel_separation_multiplier = wheel_separation_multiplier_;

dynamic_params_struct_.left_wheel_radius_multiplier = left_wheel_radius_multiplier_;
Expand Down Expand Up @@ -449,6 +461,9 @@ namespace diff_drive_controller
//
// ...
// }
pose_from_joint_position_ = dynamic_params.pose_from_joint_position;
twist_from_joint_position_ = dynamic_params.twist_from_joint_position;

wheel_separation_multiplier_ = dynamic_params.wheel_separation_multiplier;
left_wheel_radius_multiplier_ = dynamic_params.left_wheel_radius_multiplier;
right_wheel_radius_multiplier_ = dynamic_params.right_wheel_radius_multiplier;
Expand All @@ -469,51 +484,44 @@ namespace diff_drive_controller
odometry_.setMeasCovarianceParams(k_l_, k_r_);

// COMPUTE AND PUBLISH ODOMETRY
if (open_loop_)
// Read wheel joint positions and velocities:
Copy link
Member Author

Choose a reason for hiding this comment

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

I'm thinking of moving this (the reading) to the very top of the update method, so the values are readed the closest possible to the time the control cycle starts.

Now there's no problem reading before setting the dynamic params.

for (size_t i = 0; i < wheel_joints_size_; ++i)
{
odometry_.updateOpenLoop(last0_cmd_.lin, last0_cmd_.ang, time);
left_positions_[i] = left_wheel_joints_[i].getPosition();
right_positions_[i] = right_wheel_joints_[i].getPosition();

left_velocities_[i] = left_wheel_joints_[i].getVelocity();
right_velocities_[i] = right_wheel_joints_[i].getVelocity();
}
else

// Check for NaNs on wheel joint positions if we're going to use them:
use_position_ = pose_from_joint_position_ || twist_from_joint_position_;
use_velocity_ = !pose_from_joint_position_ || !twist_from_joint_position_;

if (use_position_)
{
if (position_feedback_)
for (size_t i = 0; i < wheel_joints_size_; ++i)
{
double left_pos = 0.0;
double right_pos = 0.0;
for (size_t i = 0; i < wheel_joints_size_; ++i)
if (std::isnan(left_positions_[i]) ||
std::isnan(right_positions_[i]))
{
left_positions_[i] = left_wheel_joints_[i].getPosition();
right_positions_[i] = right_wheel_joints_[i].getPosition();
if (std::isnan(left_positions_[i]) || std::isnan(right_positions_[i]))
return;

left_pos += left_positions_[i];
right_pos += right_positions_[i];
// @todo add a diagnostic message
return;
}
left_pos /= wheel_joints_size_;
right_pos /= wheel_joints_size_;

// Estimate linear and angular velocity using joint position information
odometry_.updateCloseLoop(left_pos, right_pos, time);
}
else
}

// Check for NaNs on wheel joint velocities if we're going to use them:
if (use_velocity_)
{
for (size_t i = 0; i < wheel_joints_size_; ++i)
{
double left_vel = 0.0;
double right_vel = 0.0;
for (size_t i = 0; i < wheel_joints_size_; ++i)
if (std::isnan(left_velocities_[i]) ||
std::isnan(right_velocities_[i]))
{
left_velocities_[i] = left_wheel_joints_[i].getVelocity();
right_velocities_[i] = right_wheel_joints_[i].getVelocity();
if (std::isnan(left_velocities_[i]) || std::isnan(right_velocities_[i]))
return;

left_vel += left_velocities_[i];
right_vel += right_velocities_[i];
// @todo add a diagnostic message
return;
}
left_vel /= wheel_joints_size_;
right_vel /= wheel_joints_size_;

// Estimate linear and angular velocity using joint velocity information
odometry_.updateCloseLoopFromVelocity(left_vel, right_vel, time);
}
}

Expand Down Expand Up @@ -547,10 +555,30 @@ namespace diff_drive_controller
const double left_velocity_estimated_average = std::accumulate(left_velocities_estimated_.begin(), left_velocities_estimated_.end(), 0.0) / wheel_joints_size_;
const double right_velocity_estimated_average = std::accumulate(right_velocities_estimated_.begin(), right_velocities_estimated_.end(), 0.0) / wheel_joints_size_;

// Set the wheel joint position that will be used to compute the pose:
const double left_position = pose_from_joint_position_ ? left_position_average : left_position_estimated_average;
const double right_position = pose_from_joint_position_ ? right_position_average : right_position_estimated_average;

// Set the wheel joint velocity that will be used to compute the twist:
const double left_velocity = twist_from_joint_position_ ? left_velocity_estimated_average : left_velocity_average;
const double right_velocity = twist_from_joint_position_ ? right_velocity_estimated_average : right_velocity_average;

// Update odometry:
if (open_loop_)
{
odometry_.updateOpenLoop(last0_cmd_.lin, last0_cmd_.ang, time);
}
else
{
odometry_.updateCloseLoop(left_position, right_position, left_velocity, right_velocity, time);
}

// Publish odometry message
const ros::Duration half_period(0.5 * period.toSec());
if (last_odom_publish_time_ + publish_period_ < time + half_period)
{
// @todo should be after the trylock
// and duplicate the condition for the odom and odom_tf!
last_odom_publish_time_ = time;

// Compute and store orientation info
Expand Down Expand Up @@ -602,6 +630,7 @@ 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);

Expand All @@ -620,7 +649,7 @@ namespace diff_drive_controller
}

// Publish limited velocity command:
if (dynamic_params.publish_cmd_vel_limited && cmd_vel_limited_pub_->trylock())
if (publish_cmd_vel_limited_ && cmd_vel_limited_pub_->trylock())
{
cmd_vel_limited_pub_->msg_.header.stamp = time;

Expand Down Expand Up @@ -854,6 +883,9 @@ namespace diff_drive_controller
void DiffDriveController::reconfigureCallback(
DiffDriveControllerConfig& config, uint32_t level)
{
dynamic_params_struct_.pose_from_joint_position = config.pose_from_joint_position;
dynamic_params_struct_.twist_from_joint_position = config.twist_from_joint_position;

dynamic_params_struct_.wheel_separation_multiplier = config.wheel_separation_multiplier;

dynamic_params_struct_.left_wheel_radius_multiplier = config.left_wheel_radius_multiplier;
Expand All @@ -869,6 +901,8 @@ namespace diff_drive_controller

ROS_DEBUG_STREAM_NAMED(name_,
"Reconfigured Odometry params. "
<< "pose odometry computed from: " << (dynamic_params_struct_.pose_from_joint_position ? "position" : "velocity") << ", "
<< "twist odometry computed from: " << (dynamic_params_struct_.twist_from_joint_position ? "position" : "velocity") << ", "
<< "wheel separation: " << dynamic_params_struct_.wheel_separation_multiplier << ", "
<< "left wheel radius: " << dynamic_params_struct_.left_wheel_radius_multiplier << ", "
<< "right wheel radius: " << dynamic_params_struct_.left_wheel_radius_multiplier);
Expand Down
Loading