From 22b106a31be90cddd0b34ee5c9aa55ebc06cedce Mon Sep 17 00:00:00 2001 From: Enrique Fernandez Date: Thu, 28 Jan 2016 14:20:42 -0500 Subject: [PATCH] Allow computing pose/twist from position/velocity --- .../cfg/DiffDriveController.cfg | 5 +- .../diff_drive_controller.h | 14 ++- .../include/diff_drive_controller/odometry.h | 30 ++--- .../src/diff_drive_controller.cpp | 116 +++++++++++------- diff_drive_controller/src/odometry.cpp | 79 ++++++------ .../test/diff_drive_velocity_feedback.test | 3 +- .../test/diffbot_velocity_feedback.yaml | 2 +- 7 files changed, 149 insertions(+), 100 deletions(-) diff --git a/diff_drive_controller/cfg/DiffDriveController.cfg b/diff_drive_controller/cfg/DiffDriveController.cfg index 6aacf5f36..c633ea9e1 100755 --- a/diff_drive_controller/cfg/DiffDriveController.cfg +++ b/diff_drive_controller/cfg/DiffDriveController.cfg @@ -6,6 +6,9 @@ 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) @@ -13,7 +16,7 @@ gen.add("right_wheel_radius_multiplier", double_t, 0, "Right wheel radius multi 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")) diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h index 7ec13a042..a05e37daf 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h @@ -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 left_wheel_joints_; @@ -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; @@ -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) diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.h b/diff_drive_controller/include/diff_drive_controller/odometry.h index 7932b8862..47615d121 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.h +++ b/diff_drive_controller/include/diff_drive_controller/odometry.h @@ -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 @@ -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 @@ -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_; diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 365b5aa5a..64d3a9be3 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -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) @@ -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_); @@ -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_; @@ -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; @@ -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: + 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); } } @@ -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 @@ -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); @@ -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; @@ -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; @@ -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); diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index 0222d8dbd..e86a28398 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -69,8 +69,8 @@ namespace diff_drive_controller , right_wheel_radius_(0.0) , k_l_(1.0) , k_r_(1.0) - , left_wheel_old_pos_(0.0) - , right_wheel_old_pos_(0.0) + , left_position_previous_(0.0) + , right_position_previous_(0.0) , velocity_rolling_window_size_(velocity_rolling_window_size) , v_x_acc_(RollingWindow::window_size = velocity_rolling_window_size) , v_y_acc_(RollingWindow::window_size = velocity_rolling_window_size) @@ -101,51 +101,50 @@ namespace diff_drive_controller timestamp_ = time; } - bool Odometry::updateCloseLoop(double left_pos, double right_pos, const ros::Time &time) + bool Odometry::updateCloseLoop( + const double left_position, const double right_position, + const double left_velocity, const double right_velocity, + const ros::Time &time) { - /// Estimate velocity of wheels using old and current position: - const double left_wheel_est_vel = left_pos - left_wheel_old_pos_; - const double right_wheel_est_vel = right_pos - right_wheel_old_pos_; + /// Estimate wheels position increment using previous and current position: + const double left_position_increment = left_position - left_position_previous_; + const double right_position_increment = right_position - right_position_previous_; - /// Update old position with current: - left_wheel_old_pos_ = left_pos; - right_wheel_old_pos_ = right_pos; + /// Update previous position with current: + left_position_previous_ = left_position; + right_position_previous_ = right_position; /// Update pose and twist: - return update(left_wheel_est_vel, right_wheel_est_vel, time); + return update(left_position_increment, right_position_increment, + left_velocity, right_velocity, time); } - bool Odometry::updateCloseLoopFromVelocity(double left_vel, double right_vel, const ros::Time& time) + bool Odometry::updateOpenLoop(const double linear, const double angular, + const ros::Time& time) { - /// Compute time step: - const double dt = (time - timestamp_).toSec(); - - /// Convert velocities into displacements/movements: - left_vel *= dt; - right_vel *= dt; - - /// Update pose and twist: - return update(left_vel, right_vel, time); - } - - bool Odometry::updateOpenLoop(double linear, double angular, const ros::Time& time) - { - /// Compute time step: - const double dt = (time - timestamp_).toSec(); - - /// Convert velocities into displacements/movements: - linear *= dt; - angular *= dt; - /// Compute wheel velocities, i.e. Inverse Kinematics: + // @todo we should expose a method to compute this: + // void inverseKinematics(const double linear, const double angular, + // double& left, double &right); + // note that the input/output can be velocity or relative position + // (incremental position/displacement) + // + // this method would be use here and to 'Compute wheels velocities' in + // diff_drive_controller; here is displacement, there is velocity 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_; + /// Compute time step: + const double dt = (time - timestamp_).toSec(); + /// Update pose and twist: - return update(v_l, v_r, time); + return update(v_l * dt, v_r * dt, v_l, v_r, time); } - bool Odometry::update(double v_l, double v_r, const ros::Time& time) + bool Odometry::update( + const double dp_l, const double dp_r, + const double v_l, const double v_r, + const ros::Time& time) { /// Safe current state: const SE2 p0(SE2::Scalar(heading_), SE2::Point(x_, y_)); @@ -153,10 +152,10 @@ namespace diff_drive_controller /// Integrate odometry pose: IntegrateFunction::PoseJacobian J_pose; IntegrateFunction::MeasJacobian J_meas; - (*integrate_fun_)(x_, y_, heading_, v_l, v_r, J_pose, J_meas); + (*integrate_fun_)(x_, y_, heading_, dp_l, dp_r, J_pose, J_meas); - /// Update Measurement Covariance: - updateMeasCovariance(v_l, v_r); + /// Update Measurement Covariance with the wheel joint position increments: + updateMeasCovariance(dp_l, dp_r); /// Update pose covariance: pose_covariance_ = J_pose * pose_covariance_ * J_pose.transpose() + @@ -208,15 +207,21 @@ namespace diff_drive_controller /// Integrate odometry twist: /// Note that this is done this way because it isn't trivial to compute the /// Jacobians for the relative transformation between p0 and p1 + const double dp_l = v_l * dt; + const double dp_r = v_r * dt; + IntegrateFunction::PoseJacobian J_dummy; IntegrateFunction::MeasJacobian J_meas; double x = 0.0, y = 0.0, yaw = 0.0; - (*integrate_fun_)(x, y, yaw, v_l, v_r, J_dummy, J_meas); + (*integrate_fun_)(x, y, yaw, dp_l, dp_r, J_dummy, J_meas); /// Include the Jacobian of dividing by dt, which is equivalent to divide /// all the elements of the other Jacobian by dt: J_meas /= dt; + /// Update Measurement Covariance with the wheel joint velocites: + updateMeasCovariance(dp_l, dp_r); + /// Update twist covariance: twist_covariance_ = J_meas * meas_covariance_ * J_meas.transpose(); diff --git a/diff_drive_controller/test/diff_drive_velocity_feedback.test b/diff_drive_controller/test/diff_drive_velocity_feedback.test index 41fb85892..1cedb85e3 100644 --- a/diff_drive_controller/test/diff_drive_velocity_feedback.test +++ b/diff_drive_controller/test/diff_drive_velocity_feedback.test @@ -2,7 +2,8 @@ - + diff --git a/diff_drive_controller/test/diffbot_velocity_feedback.yaml b/diff_drive_controller/test/diffbot_velocity_feedback.yaml index 2c596c84b..c4bc58e24 100644 --- a/diff_drive_controller/test/diffbot_velocity_feedback.yaml +++ b/diff_drive_controller/test/diffbot_velocity_feedback.yaml @@ -1,2 +1,2 @@ diffbot_controller: - position_feedback: false + pose_from_joint_position: false