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

Open-loop odometry #39

Merged
merged 4 commits into from
Jul 21, 2014
Merged

Open-loop odometry #39

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
a482cea
adds open loop odometry
Jul 18, 2014
a8fea93
Cosmetic refactoring
bmagyar Jul 18, 2014
b97b942
adds open loop test
efernandez Jul 20, 2014
1f846b6
adds odometry init to reset timestamp and accs
Jul 21, 2014
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/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,9 @@ if (CATKIN_ENABLE_TESTING)

add_dependencies(tests diffbot)

add_rostest_gtest(diff_drive_test test/diff_drive_controller.test test/diff_drive_test.cpp)
add_rostest_gtest(diff_drive_test
test/diff_drive_controller.test
test/diff_drive_test.cpp)
target_link_libraries(diff_drive_test ${catkin_LIBRARIES})
add_rostest_gtest(diff_drive_nan_test
test/diff_drive_controller_nan.test
Expand All @@ -58,5 +60,6 @@ if (CATKIN_ENABLE_TESTING)
test/diff_drive_fail_test.cpp)
target_link_libraries(diff_drive_fail_test ${catkin_LIBRARIES})
add_rostest(test/diff_drive_bad_urdf.test)
add_rostest(test/diff_drive_open_loop.test)

endif()
6 changes: 3 additions & 3 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 @@ -98,9 +98,10 @@ namespace diff_drive_controller{
private:
std::string name_;

/// Publish rate related:
/// Odometry related:
ros::Duration publish_period_;
ros::Time last_state_publish_time_;
bool open_loop_;

/// Hardware handles:
hardware_interface::JointHandle left_wheel_joint_;
Expand All @@ -123,7 +124,6 @@ namespace diff_drive_controller{
boost::shared_ptr<realtime_tools::RealtimePublisher<nav_msgs::Odometry> > odom_pub_;
boost::shared_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > tf_odom_pub_;
Odometry odometry_;
geometry_msgs::TransformStamped odom_frame_;

/// Wheel separation, wrt the midpoint of the wheel width:
double wheel_separation_;
Expand All @@ -141,7 +141,7 @@ namespace diff_drive_controller{
/// Frame to use for the robot base:
std::string base_frame_id_;

// speed limiters
// Speed limiters:
Commands last_cmd_;
SpeedLimiter limiter_lin_;
SpeedLimiter limiter_ang_;
Expand Down
48 changes: 33 additions & 15 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 @@ -35,6 +35,8 @@
/*
* Author: Luca Marchionni
* Author: Bence Magyar
* Author: Enrique Fernández
* Author: Paul Mathieu
*/

#ifndef ODOMETRY_H_
Expand Down Expand Up @@ -69,6 +71,12 @@ namespace diff_drive_controller
*/
Odometry(size_t velocity_rolling_window_size = 10);

/**
* \brief Initialize the odometry
* \param time Current time
*/
void init(const ros::Time &time);

/**
* \brief Updates the odometry class with latest wheels position
* \param left_pos Left wheel position [rad]
Expand All @@ -78,6 +86,14 @@ namespace diff_drive_controller
*/
bool update(double left_pos, double right_pos, const ros::Time &time);

/**
* \brief Updates the odometry class with latest velocity command
* \param linear Linear velocity [m/s]
* \param angular Angular velocity [rad/s]
* \param time Current time
*/
void updateOpenLoop(double linear, double angular, const ros::Time &time);

/**
* \brief heading getter
* \return heading [rad]
Expand All @@ -98,33 +114,30 @@ namespace diff_drive_controller

/**
* \brief y position getter
* \return y positioin [m]
* \return y position [m]
*/
double getY() const
{
return y_;
}

/**
* \brief timestamp getter
* \return timestamp
* \brief linear velocity getter
* \return linear velocity [m/s]
*/
ros::Time getTimestamp() const
double getLinear() const
{
return timestamp_;
return linear_;
}

/**
* \brief Retrieves the linear velocity estimation
* \return Rolling mean estimation of the linear velocity [m]
* \brief angular velocity getter
* \return angular velocity [rad/s]
*/
double getLinearEstimated() const;

/**
* \brief Retrieves the angular velocity estimation
* \return Rolling mean estimation of the angular velocity [rad/s]
*/
double getAngularEstimated() const;
double getAngular() const
{
return angular_;
}

/**
* \brief Sets the wheel parameters: radius and separation
Expand Down Expand Up @@ -161,6 +174,10 @@ namespace diff_drive_controller
double y_; // [m]
double heading_; // [rad]

/// Current velocity:
double linear_; // [m/s]
double angular_; // [rad/s]

/// Wheel kinematic parameters [m]:
double wheel_separation_;
double wheel_radius_;
Expand All @@ -169,7 +186,8 @@ namespace diff_drive_controller
double left_wheel_old_pos_;
double right_wheel_old_pos_;

/// Rolling meand accumulators for the linar and angular velocities:
/// Rolling mean accumulators for the linar and angular velocities:
size_t velocity_rolling_window_size_;
RollingMeanAcc linear_acc_;
RollingMeanAcc angular_acc_;

Expand Down
74 changes: 44 additions & 30 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 @@ -108,7 +108,8 @@ static bool getWheelRadius(const boost::shared_ptr<const urdf::Link>& wheel_link
namespace diff_drive_controller{

DiffDriveController::DiffDriveController()
: command_struct_()
: open_loop_(false)
Copy link

Choose a reason for hiding this comment

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

It is better style to add new parameters at the bottom. This can help prevent ABI breaks.

Copy link
Author

Choose a reason for hiding this comment

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

I tried to put it together with other odometry related attributes in the header file.
It just happened to be before command_struct_, so I had to put it at the beginning.

Copy link

Choose a reason for hiding this comment

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

no problem. This is not really a library anyway, so nothing builds against this API.

, command_struct_()
, wheel_separation_(0.0)
, wheel_radius_(0.0)
, wheel_separation_multiplier_(1.0)
Expand Down Expand Up @@ -141,12 +142,15 @@ namespace diff_drive_controller{
return false;
}

// Odometry related:
double publish_rate;
controller_nh.param("publish_rate", publish_rate, 50.0);
ROS_INFO_STREAM_NAMED(name_, "Controller state will be published at "
<< publish_rate << "Hz.");
publish_period_ = ros::Duration(1.0 / publish_rate);

controller_nh.param("open_loop", open_loop_, open_loop_);

controller_nh.param("wheel_separation_multiplier", wheel_separation_multiplier_, wheel_separation_multiplier_);
ROS_INFO_STREAM_NAMED(name_, "Wheel separation will be multiplied by "
<< wheel_separation_multiplier_ << ".");
Expand All @@ -155,6 +159,7 @@ namespace diff_drive_controller{
ROS_INFO_STREAM_NAMED(name_, "Wheel radius will be multiplied by "
<< wheel_radius_multiplier_ << ".");

// Twist command related:
controller_nh.param("cmd_vel_timeout", cmd_vel_timeout_, cmd_vel_timeout_);
ROS_INFO_STREAM_NAMED(name_, "Velocity commands will be considered old if they are older than "
<< cmd_vel_timeout_ << "s.");
Expand Down Expand Up @@ -197,13 +202,20 @@ namespace diff_drive_controller{
void DiffDriveController::update(const ros::Time& time, const ros::Duration& period)
{
// COMPUTE AND PUBLISH ODOMETRY
const double left_pos = left_wheel_joint_.getPosition();
const double right_pos = right_wheel_joint_.getPosition();
if (std::isnan(left_pos) || std::isnan(right_pos))
return;
if (open_loop_)
{
odometry_.updateOpenLoop(last_cmd_.lin, last_cmd_.ang, time);
}
else
{
const double left_pos = left_wheel_joint_.getPosition();
double right_pos = right_wheel_joint_.getPosition();
if (std::isnan(left_pos) || std::isnan(right_pos))
return;

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

// Publish odometry message
if (last_state_publish_time_ + publish_period_ < time)
Expand All @@ -220,19 +232,19 @@ namespace diff_drive_controller{
odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
odom_pub_->msg_.pose.pose.orientation = orientation;
odom_pub_->msg_.twist.twist.linear.x = odometry_.getLinearEstimated();
odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngularEstimated();
odom_pub_->msg_.twist.twist.linear.x = odometry_.getLinear();
odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
odom_pub_->unlockAndPublish();
}

// Publish tf /odom frame
if (tf_odom_pub_->trylock())
{
odom_frame_.header.stamp = time;
odom_frame_.transform.translation.x = odometry_.getX();
odom_frame_.transform.translation.y = odometry_.getY();
odom_frame_.transform.rotation = orientation;
tf_odom_pub_->msg_.transforms[0] = odom_frame_;
geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0];
odom_frame.header.stamp = time;
odom_frame.transform.translation.x = odometry_.getX();
odom_frame.transform.translation.y = odometry_.getY();
odom_frame.transform.rotation = orientation;
tf_odom_pub_->unlockAndPublish();
}
}
Expand All @@ -250,7 +262,7 @@ namespace diff_drive_controller{
}

// Limit velocities and accelerations:
double cmd_dt = period.toSec();
const double cmd_dt(period.toSec());
limiter_lin_.limit(curr_cmd.lin, last_cmd_.lin, cmd_dt);
limiter_ang_.limit(curr_cmd.ang, last_cmd_.ang, cmd_dt);
last_cmd_ = curr_cmd;
Expand All @@ -274,6 +286,8 @@ namespace diff_drive_controller{

// Register starting time used to keep fixed rate
last_state_publish_time_ = time;

odometry_.init(time);
}

void DiffDriveController::stopping(const ros::Time& time)
Expand Down Expand Up @@ -390,28 +404,28 @@ namespace diff_drive_controller{
odom_pub_->msg_.child_frame_id = base_frame_id_;
odom_pub_->msg_.pose.pose.position.z = 0;
odom_pub_->msg_.pose.covariance = boost::assign::list_of
(static_cast<double>(pose_cov_list[0])) (0) (0) (0) (0) (0)
(0) (static_cast<double>(pose_cov_list[1])) (0) (0) (0) (0)
(0) (0) (static_cast<double>(pose_cov_list[2])) (0) (0) (0)
(0) (0) (0) (static_cast<double>(pose_cov_list[3])) (0) (0)
(0) (0) (0) (0) (static_cast<double>(pose_cov_list[4])) (0)
(0) (0) (0) (0) (0) (static_cast<double>(pose_cov_list[5]));
(static_cast<double>(pose_cov_list[0])) (0) (0) (0) (0) (0)
(0) (static_cast<double>(pose_cov_list[1])) (0) (0) (0) (0)
(0) (0) (static_cast<double>(pose_cov_list[2])) (0) (0) (0)
(0) (0) (0) (static_cast<double>(pose_cov_list[3])) (0) (0)
(0) (0) (0) (0) (static_cast<double>(pose_cov_list[4])) (0)
(0) (0) (0) (0) (0) (static_cast<double>(pose_cov_list[5]));
odom_pub_->msg_.twist.twist.linear.y = 0;
odom_pub_->msg_.twist.twist.linear.z = 0;
odom_pub_->msg_.twist.twist.angular.x = 0;
odom_pub_->msg_.twist.twist.angular.y = 0;
odom_pub_->msg_.twist.covariance = boost::assign::list_of
(static_cast<double>(twist_cov_list[0])) (0) (0) (0) (0) (0)
(0) (static_cast<double>(twist_cov_list[1])) (0) (0) (0) (0)
(0) (0) (static_cast<double>(twist_cov_list[2])) (0) (0) (0)
(0) (0) (0) (static_cast<double>(twist_cov_list[3])) (0) (0)
(0) (0) (0) (0) (static_cast<double>(twist_cov_list[4])) (0)
(0) (0) (0) (0) (0) (static_cast<double>(twist_cov_list[5]));
(static_cast<double>(twist_cov_list[0])) (0) (0) (0) (0) (0)
(0) (static_cast<double>(twist_cov_list[1])) (0) (0) (0) (0)
(0) (0) (static_cast<double>(twist_cov_list[2])) (0) (0) (0)
(0) (0) (0) (static_cast<double>(twist_cov_list[3])) (0) (0)
(0) (0) (0) (0) (static_cast<double>(twist_cov_list[4])) (0)
(0) (0) (0) (0) (0) (static_cast<double>(twist_cov_list[5]));
tf_odom_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(root_nh, "/tf", 100));
tf_odom_pub_->msg_.transforms.resize(1);
odom_frame_.transform.translation.z = 0.0;
odom_frame_.child_frame_id = base_frame_id_;
odom_frame_.header.frame_id = "odom";
tf_odom_pub_->msg_.transforms[0].transform.translation.z = 0.0;
tf_odom_pub_->msg_.transforms[0].child_frame_id = base_frame_id_;
tf_odom_pub_->msg_.transforms[0].header.frame_id = "odom";
}

} // namespace diff_drive_controller
55 changes: 38 additions & 17 deletions diff_drive_controller/src/odometry.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,10 @@
*********************************************************************/

/*
* Author: Luca Marchionni
* Author: Bence Magyar
* Author: Enrique Fernández
* Author: Paul Mathieu
*/

#include <diff_drive_controller/odometry.h>
Expand All @@ -46,20 +49,33 @@ namespace diff_drive_controller
namespace bacc = boost::accumulators;

Odometry::Odometry(size_t velocity_rolling_window_size)
: timestamp_(0.0),
x_(0.0),
y_(0.0),
heading_(0.0),
wheel_separation_(0.0),
wheel_radius_(0.0),
left_wheel_old_pos_(0.0),
right_wheel_old_pos_(0.0),
linear_acc_(RollingWindow::window_size = velocity_rolling_window_size),
angular_acc_(RollingWindow::window_size = velocity_rolling_window_size),
integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2))
: timestamp_(0.0)
, x_(0.0)
, y_(0.0)
, heading_(0.0)
, linear_(0.0)
, angular_(0.0)
, wheel_separation_(0.0)
, wheel_radius_(0.0)
, left_wheel_old_pos_(0.0)
, right_wheel_old_pos_(0.0)
, velocity_rolling_window_size_(velocity_rolling_window_size)
, linear_acc_(RollingWindow::window_size = velocity_rolling_window_size)
, angular_acc_(RollingWindow::window_size = velocity_rolling_window_size)
, integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2))
{
}

void Odometry::init(const ros::Time& time)
{
// Reset accumulators:
linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);

// Reset timestamp:
timestamp_ = time;
}

bool Odometry::update(double left_pos, double right_pos, const ros::Time &time)
{
/// Get current wheel joint positions:
Expand Down Expand Up @@ -92,17 +108,22 @@ namespace diff_drive_controller
linear_acc_(linear/dt);
angular_acc_(angular/dt);

linear_ = bacc::rolling_mean(linear_acc_);
angular_ = bacc::rolling_mean(angular_acc_);

return true;
}

double Odometry::getLinearEstimated() const
void Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time)
{
return bacc::rolling_mean(linear_acc_);
}
/// Save last linear and angular velocity:
linear_ = linear;
angular_ = angular;

double Odometry::getAngularEstimated() const
{
return bacc::rolling_mean(angular_acc_);
/// Integrate odometry:
const double dt = (time - timestamp_).toSec();
timestamp_ = time;
integrate_fun_(linear * dt, angular * dt);
}

void Odometry::setWheelParams(double wheel_separation, double wheel_radius)
Expand Down
Loading