HiepLM/AMR_T800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit for v2"

Browse Source
This commit is contained in:
HiepLM
2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

173
Devices/Packages/ros_kinematics/src/odometry.cpp Executable file
View File

@@ -0,0 +1,173 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2013, PAL Robotics, S.L.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the PAL Robotics nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/*
* Author: Luca Marchionni
* Author: Bence Magyar
* Author: Enrique Fernández
* Author: Paul Mathieu
*/
#include <ros/ros.h>
#include <ros_kinematics/odometry.h>
namespace ros_kinematics
{
namespace bacc = boost::accumulators;
Odometry::Odometry(size_t velocity_rolling_window_size)
: timestamp_(0.0)
, x_(0.0)
, y_(0.0)
, heading_(0.0)
, linear_(0.0)
, angular_(0.0)
, wheel_separation_(0.0)
, left_wheel_radius_(0.0)
, right_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_(std::bind(&Odometry::integrateExact, this, std::placeholders::_1, std::placeholders::_2))
{
}
void Odometry::init(const ros::Time& time)
{
// Reset accumulators and timestamp:
resetAccumulators();
timestamp_ = time;
}
bool Odometry::update(double left_pos, double right_pos, const ros::Time &time)
{
/// Get current wheel joint positions:
const double left_wheel_cur_pos = left_pos * left_wheel_radius_;
const double right_wheel_cur_pos = right_pos * right_wheel_radius_;
/// Estimate velocity of wheels using old and current position:
const double left_wheel_est_vel = left_wheel_cur_pos - left_wheel_old_pos_;
const double right_wheel_est_vel = right_wheel_cur_pos - right_wheel_old_pos_;
/// Update old position with current:
left_wheel_old_pos_ = left_wheel_cur_pos;
right_wheel_old_pos_ = right_wheel_cur_pos;
/// Compute linear and angular diff:
const double linear = (right_wheel_est_vel + left_wheel_est_vel) * 0.5 ;
const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_;
/// Integrate odometry:
integrate_fun_(linear, angular);
/// We cannot estimate the speed with very small time intervals:
const double dt = (time - timestamp_).toSec();
if (dt < 0.0001)
return false; // Interval too small to integrate with
timestamp_ = time;
/// Estimate speeds using a rolling mean to filter them out:
linear_acc_(linear/dt);
angular_acc_(angular/dt);
linear_ = bacc::rolling_mean(linear_acc_);
angular_ = bacc::rolling_mean(angular_acc_);
return true;
}
void Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time)
{
/// Save last linear and angular velocity:
linear_ = linear;
angular_ = angular;
/// Integrate odometry:
const double dt = (time - timestamp_).toSec();
timestamp_ = time;
integrate_fun_(linear * dt, angular * dt);
}
void Odometry::setWheelParams(double wheel_separation, double left_wheel_radius, double right_wheel_radius)
{
wheel_separation_ = wheel_separation;
left_wheel_radius_ = left_wheel_radius;
right_wheel_radius_ = right_wheel_radius;
}
void Odometry::setVelocityRollingWindowSize(size_t velocity_rolling_window_size)
{
velocity_rolling_window_size_ = velocity_rolling_window_size;
resetAccumulators();
}
void Odometry::integrateRungeKutta2(double linear, double angular)
{
const double direction = heading_ + angular * 0.5;
/// Runge-Kutta 2nd order integration:
x_ += linear * cos(direction);
y_ += linear * sin(direction);
heading_ += angular;
}
/**
* \brief Other possible integration method provided by the class
* \param linear
* \param angular
*/
void Odometry::integrateExact(double linear, double angular)
{
if (fabs(angular) < 1e-6)
integrateRungeKutta2(linear, angular);
else
{
/// Exact integration (should solve problems when angular is zero):
const double heading_old = heading_;
const double r = linear/angular;
heading_ += angular;
x_ += r * (sin(heading_) - sin(heading_old));
y_ += -r * (cos(heading_) - cos(heading_old));
}
}
void Odometry::resetAccumulators()
{
linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_);
}
} // namespace ros_kinematics

824
Devices/Packages/ros_kinematics/src/ros_diff_drive.cpp Executable file
View File

@@ -0,0 +1,824 @@
#include "ros_kinematics/ros_diff_drive.h"
#include <std_msgs/UInt16.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>
ros_kinematics::RosDiffDrive::RosDiffDrive(ros::NodeHandle& nh, const std::string& name)
: nh_(nh),
name_(name),
motor_loader_("models", "models::BaseSteerDrive"),
encoder_loader_("models", "models::BaseAbsoluteEncoder"),
last_odom_state_publish_time_(ros::Time(0)),
left_wheel_curr_pos_(0),
right_wheel_curr_pos_(0),
publish_time_(ros::Time(0)),
initialized_(false)
{
if(!initialized_)
{
nh_priv_ = ros::NodeHandle(nh_, name_);
ROS_INFO("RosDiffDrive get node name is %s", name_.c_str());
bool init_odom_enc;
nh_priv_.param("initialize_odom_enc", init_odom_enc, true);
odom_enc_initialized_ = !init_odom_enc;
nh_priv_.param("wheel_separation_multiplier", wheel_separation_multiplier_, wheel_separation_multiplier_);
ROS_INFO_STREAM_NAMED(name_, "Wheel separation will be multiplied by " << wheel_separation_multiplier_ << ".");
if (nh_priv_.hasParam("wheel_radius_multiplier"))
{
double wheel_radius_multiplier;
nh_priv_.getParam("wheel_radius_multiplier", wheel_radius_multiplier);
left_wheel_radius_multiplier_ = wheel_radius_multiplier;
right_wheel_radius_multiplier_ = wheel_radius_multiplier;
}
else
{
nh_priv_.param("left_wheel_radius_multiplier", left_wheel_radius_multiplier_, left_wheel_radius_multiplier_);
nh_priv_.param("right_wheel_radius_multiplier", right_wheel_radius_multiplier_, right_wheel_radius_multiplier_);
}
ROS_INFO_STREAM_NAMED(name_, "Left wheel radius will be multiplied by " << left_wheel_radius_multiplier_ << ".");
ROS_INFO_STREAM_NAMED(name_, "Right wheel radius will be multiplied by " << right_wheel_radius_multiplier_ << ".");
int velocity_rolling_window_size = 10;
nh_priv_.param("velocity_rolling_window_size", velocity_rolling_window_size, velocity_rolling_window_size);
ROS_INFO_STREAM_NAMED(name_, "Velocity rolling window size of " << velocity_rolling_window_size << ".");
odometry_.setVelocityRollingWindowSize(velocity_rolling_window_size);
// Twist command related:
nh_priv_.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.");
nh_priv_.param("allow_multiple_cmd_vel_publishers", allow_multiple_cmd_vel_publishers_, allow_multiple_cmd_vel_publishers_);
ROS_INFO_STREAM_NAMED(name_, "Allow mutiple cmd_vel publishers is " << (allow_multiple_cmd_vel_publishers_ ? "enabled" : "disabled"));
nh_priv_.param("base_frame_id", base_frame_id_, base_frame_id_);
ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_frame_id_);
nh_priv_.param("odom_frame_id", odom_frame_id_, odom_frame_id_);
ROS_INFO_STREAM_NAMED(name_, "Odometry frame_id set to " << odom_frame_id_);
nh_priv_.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_);
ROS_INFO_STREAM_NAMED(name_, "Publishing base frame to tf is " << (enable_odom_tf_ ? "enabled" : "disabled"));
nh_priv_.param("enable_wheel_tf", enable_wheel_tf_, enable_wheel_tf_);
ROS_INFO_STREAM_NAMED(name_, "Publishing wheel frame to tf is " << (enable_wheel_tf_ ? "enabled" : "disabled"));
nh_priv_.param("commandTopic", command_topic_, std::string("cmd_vel"));
// nh_priv_.param("publishOdomTF", publishOdomTF_, false);
nh_priv_.param("odometryTopic", odometry_topic_, std::string("odom"));
// nh_priv_.param("odometryFrame", odometry_frame_, std::string("odom"));
nh_priv_.param("odometryEncTopic", odometry_enc_topic_, std::string("odom_enc"));
nh_priv_.param("odometryEncChildFrame", odometry_enc_child_frame_, std::string("odom_enc"));
// nh_priv_.param("robotBaseFrame", robot_base_frame_, std::string("base_link"));
nh_priv_.param("subscribe_queue_size", m_subscribe_queue_size_, 1);
nh_priv_.param("publish_rate", update_rate_, 100.0);
publish_period_ = ros::Duration(1.0 / update_rate_);
nh_priv_.param("open_loop", open_loop_, false);
// Velocity and acceleration limits:
nh_priv_.param("linear/x/has_velocity_limits", limiter_lin_.has_velocity_limits, limiter_lin_.has_velocity_limits);
nh_priv_.param("linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits);
nh_priv_.param("linear/x/has_jerk_limits", limiter_lin_.has_jerk_limits, limiter_lin_.has_jerk_limits);
nh_priv_.param("linear/x/max_velocity", limiter_lin_.max_velocity, limiter_lin_.max_velocity);
nh_priv_.param("linear/x/min_velocity", limiter_lin_.min_velocity, -limiter_lin_.max_velocity);
nh_priv_.param("linear/x/max_acceleration", limiter_lin_.max_acceleration, limiter_lin_.max_acceleration);
nh_priv_.param("linear/x/min_acceleration", limiter_lin_.min_acceleration, -limiter_lin_.max_acceleration);
nh_priv_.param("linear/x/max_jerk", limiter_lin_.max_jerk, limiter_lin_.max_jerk);
nh_priv_.param("linear/x/min_jerk", limiter_lin_.min_jerk, -limiter_lin_.max_jerk);
nh_priv_.param("angular/z/has_velocity_limits", limiter_ang_.has_velocity_limits, limiter_ang_.has_velocity_limits);
nh_priv_.param("angular/z/has_acceleration_limits", limiter_ang_.has_acceleration_limits, limiter_ang_.has_acceleration_limits);
nh_priv_.param("angular/z/has_jerk_limits", limiter_ang_.has_jerk_limits, limiter_ang_.has_jerk_limits);
nh_priv_.param("angular/z/max_velocity", limiter_ang_.max_velocity, limiter_ang_.max_velocity);
nh_priv_.param("angular/z/min_velocity", limiter_ang_.min_velocity, -limiter_ang_.max_velocity);
nh_priv_.param("angular/z/max_acceleration", limiter_ang_.max_acceleration, limiter_ang_.max_acceleration);
nh_priv_.param("angular/z/min_acceleration", limiter_ang_.min_acceleration, -limiter_ang_.max_acceleration);
nh_priv_.param("angular/z/max_jerk", limiter_ang_.max_jerk, limiter_ang_.max_jerk);
nh_priv_.param("angular/z/min_jerk", limiter_ang_.min_jerk, -limiter_ang_.max_jerk);
// Publish limited velocity:
nh_priv_.param("publish_cmd", publish_cmd_, publish_cmd_);
// Use encoder
nh_.param(name_ + "/use_encoder", use_abs_encoder, false);
// If either parameter is not available, we need to look up the value in the URDF
bool lookup_wheel_separation = !nh_priv_.getParam("wheel_separation", wheel_separation_);
bool lookup_wheel_radius = !nh_priv_.getParam("wheel_radius", wheel_radius_);
if(lookup_wheel_separation || lookup_wheel_radius)
ROS_INFO("URDF if not specified as a parameter. We don't set the value here \n wheel_separation: %f, wheel_radius %f",
wheel_separation_, wheel_radius_);
// Regardless of how we got the separation and radius, use them
// to set the odometry parameters
const double ws = wheel_separation_multiplier_ * wheel_separation_;
const double lwr = left_wheel_radius_multiplier_ * wheel_radius_;
const double rwr = right_wheel_radius_multiplier_ * wheel_radius_;
odometry_.setWheelParams(ws, lwr, rwr);
ROS_INFO_STREAM_NAMED(name_,
"Odometry params : wheel separation " << ws
<< ", left wheel radius " << lwr
<< ", right wheel radius " << rwr);
if (publish_cmd_)
{
cmd_vel_pub_.reset(new realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped>(nh_priv_, "cmd_vel_out", 100));
}
// Get joint names from the parameter server
std::vector<std::string> left_wheel_names, right_wheel_names;
if (!getWheelNames(nh_priv_, "left_wheel", left_wheel_names) ||
!getWheelNames(nh_priv_, "right_wheel", right_wheel_names))
{
exit(1);
}
if (!getWheelParams(nh_priv_, left_wheel_names, left_drive_param_map_) ||
!getWheelParams(nh_priv_, right_wheel_names, right_drive_param_map_))
{
exit(1);
}
if (left_wheel_names.size() != right_wheel_names.size() ||
left_drive_param_map_.size() != left_wheel_names.size() ||
right_drive_param_map_.size() != right_wheel_names.size()
)
{
ROS_ERROR_STREAM_NAMED(name_,
"#left wheels (" << left_wheel_names.size() << ") != " <<
"#right wheels (" << right_wheel_names.size() << ").");
exit(1);
}
else
{
wheel_joints_size_ = left_wheel_names.size();
left_drive_.resize(wheel_joints_size_);
right_drive_.resize(wheel_joints_size_);
if (use_abs_encoder)
{
left_encoder_.resize(wheel_joints_size_);
right_encoder_.resize(wheel_joints_size_);
}
}
// plugin models
ROS_INFO("RosDiffDrive is plugining models...");
for (size_t i = 0; i < wheel_joints_size_; ++i)
{
std::string left_drive;
nh_priv_.param(left_wheel_names[i] + "/lookup_name", left_drive, std::string("motor_wheel::LeftMotor"));
try
{
left_drive_[i] = motor_loader_.createInstance(left_drive);
// left_drive_[i]->initialize(nh_, name_ + "/" + motor_loader_.getName(left_drive));
left_drive_[i]->initialize(nh_, name_ + "/" + left_wheel_names[i]);
if (use_abs_encoder)
{
std::string right_encoder;
nh_priv_.param(right_wheel_names[i] + "/encoder/lookup_name", right_encoder, std::string("absolute_encoder::Measurements"));
try
{
right_encoder_[i] = encoder_loader_.createInstance(right_encoder);
right_encoder_[i]->initialize(nh_, name_ + "/" + right_wheel_names[i] + "/encoder");
}
catch (const pluginlib::PluginlibException& ex)
{
ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s",
right_encoder.c_str(), ex.what());
exit(1);
}
}
}
catch (const pluginlib::PluginlibException& ex)
{
ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s",
left_drive.c_str(), ex.what());
exit(1);
}
std::string right_drive;
nh_priv_.param(right_wheel_names[i] + "/lookup_name", right_drive, std::string("motor_wheel::RightMotor"));
try
{
right_drive_[i] = motor_loader_.createInstance(right_drive);
// right_drive_[i]->initialize(nh_, name_ + "/" + motor_loader_.getName(right_drive));
right_drive_[i]->initialize(nh_, name_ + "/" + right_wheel_names[i]);
if (use_abs_encoder)
{
std::string left_encoder;
nh_priv_.param(left_wheel_names[i] + "/encoder/lookup_name", left_encoder, std::string("absolute_encoder::Measurements"));
try
{
left_encoder_[i] = encoder_loader_.createInstance(left_encoder);
left_encoder_[i]->initialize(nh_, name_ + "/" + left_wheel_names[i] + "/encoder");
}
catch (const pluginlib::PluginlibException& ex)
{
ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s",
left_encoder.c_str(), ex.what());
exit(1);
}
}
}
catch (const pluginlib::PluginlibException& ex)
{
ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s",
right_drive.c_str(), ex.what());
exit(1);
}
}
if(enable_wheel_tf_)
{
left_wheel_tf_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(nh_, "/tf", 100));
for (std::map<std::string, XmlRpc::XmlRpcValue>::iterator itr = left_drive_param_map_.begin(); itr != left_drive_param_map_.end(); ++itr)
{
geometry_msgs::TransformStamped tfs;
tfs.child_frame_id = static_cast<std::string>(itr->second["frame_id"]);
tfs.header.frame_id = "base_link";
tfs.transform.translation.x = static_cast<double>(itr->second["origin"][0]);
tfs.transform.translation.y = static_cast<double>(itr->second["origin"][1]);
tfs.transform.translation.z = static_cast<double>(itr->second["origin"][2]);
double roll = static_cast<double>(itr->second["origin"][3]);
double pitch = static_cast<double>(itr->second["origin"][4]);
double yaw = static_cast<double>(itr->second["origin"][5]);
geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
tfs.transform.rotation = q;
left_wheel_tf_pub_->msg_.transforms.push_back(tfs);
}
right_wheel_tf_pub_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(nh_, "/tf", 100));
for (std::map<std::string, XmlRpc::XmlRpcValue>::iterator itr = right_drive_param_map_.begin(); itr != right_drive_param_map_.end(); ++itr)
{
geometry_msgs::TransformStamped tfs;
tfs.child_frame_id = static_cast<std::string>(itr->second["frame_id"]);
tfs.header.frame_id = "base_link";
tfs.transform.translation.x = static_cast<double>(itr->second["origin"][0]);
tfs.transform.translation.y = static_cast<double>(itr->second["origin"][1]);
tfs.transform.translation.z = static_cast<double>(itr->second["origin"][2]);
double roll = static_cast<double>(itr->second["origin"][3]);
double pitch = static_cast<double>(itr->second["origin"][4]);
double yaw = static_cast<double>(itr->second["origin"][5]);
geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
tfs.transform.rotation = q;
right_wheel_tf_pub_->msg_.transforms.push_back(tfs);
}
}
schedule_delay_ = ros::Duration(1/update_rate_);
schedule_lastest_delay_ = ros::Duration(0.5);
this->setOdomPubFields(nh_, nh_priv_);
transform_broadcaster_ = boost::shared_ptr<tf2_ros::TransformBroadcaster>(new tf2_ros::TransformBroadcaster());
cmd_vel_subscriber_ = nh_.subscribe(command_topic_, m_subscribe_queue_size_, &ros_kinematics::RosDiffDrive::CmdVelCallback, this);
// Kinematics
kinematics_param_ptr_ = boost::make_shared<ros_kinematics::KinematicDiffDriveParameters>();
kinematics_param_ptr_->initialize(nh_);
// Thread
callback_thread_ = boost::make_shared<boost::thread>(&ros_kinematics::RosDiffDrive::updateChild, this);
ROS_INFO_NAMED("RosDiffDrive","Initializing on %s is successed", name_.c_str());
initialized_ = true;
}
}
ros_kinematics::RosDiffDrive::~RosDiffDrive()
{
callback_thread_ = nullptr;
kinematics_param_ptr_ = nullptr;
}
void ros_kinematics::RosDiffDrive::CmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg)
{
// check that we don't have multiple publishers on the command topic
if (!allow_multiple_cmd_vel_publishers_ && cmd_vel_subscriber_.getNumPublishers() > 1)
{
ROS_ERROR_STREAM_THROTTLE_NAMED(1.0, name_, "Detected " << cmd_vel_subscriber_.getNumPublishers()
<< " publishers " << command_topic_ << ". Only 1 publisher is allowed. Going to brake.");
brake();
return;
}
if (!std::isfinite(cmd_msg->angular.z) || !std::isfinite(cmd_msg->linear.x))
{
ROS_WARN_THROTTLE(1.0, "Received NaN in velocity command. Ignoring.");
return;
}
command_struct_.angular.z = cmd_msg->angular.z;
command_struct_.linear.x = cmd_msg->linear.x;
command_struct_.stamp = ros::Time::now();
command_.writeFromNonRT(command_struct_);
this->scheduleMotorController(true);
ROS_DEBUG_STREAM_NAMED(name_,
"Added values to command. "
<< "Ang: " << command_struct_.angular.z << ", "
<< "Lin: " << command_struct_.linear.x << ", "
<< "Stamp: " << command_struct_.stamp);
}
bool ros_kinematics::RosDiffDrive::isCmdVelTriggered(void)
{
boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
return !publish_time_.isZero() && ros::Time::now() > publish_time_;
}
bool ros_kinematics::RosDiffDrive::isCmdVelLastestTriggered(void)
{
boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
return !publish_lastest_time_.isZero() && ros::Time::now() > publish_lastest_time_;
}
void ros_kinematics::RosDiffDrive::scheduleMotorController(bool schedule)
{
boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
if(schedule && publish_time_.isZero())
{
publish_time_ = ros::Time::now() + schedule_delay_;
publish_lastest_time_ = ros::Time::now() + schedule_lastest_delay_;
}
if(!schedule && !publish_time_.isZero())
{
publish_time_ = ros::Time(0);
publish_lastest_time_ = ros::Time(0);
}
}
void ros_kinematics::RosDiffDrive::updateOdometryEncoder(const ros::Time& current_time)
{
// ros::Time current_time = ros::Time::now();
double step_time = ros::Duration(current_time - last_odom_update_).toSec();
last_odom_update_ = current_time;
// Apply (possibly new) multipliers:
const double ws = wheel_separation_multiplier_ * wheel_separation_;
const double lwr = left_wheel_radius_multiplier_ * wheel_radius_;
const double rwr = right_wheel_radius_multiplier_ * wheel_radius_;
odometry_.setWheelParams(ws, lwr, rwr);
// COMPUTE AND PUBLISH ODOMETRY
if (open_loop_)
{
odometry_.updateOpenLoop(last0_cmd_.linear.x, last0_cmd_.angular.z, current_time);
}
else
{
double left_pos = 0.0;
double right_pos = 0.0;
for (size_t i = 0; i < wheel_joints_size_; ++i)
{
double left_speed, right_speed;
if (use_abs_encoder)
{
left_speed = left_encoder_[i]->Velocity();
right_speed = right_encoder_[i]->Velocity();
}
else
{
left_speed = left_drive_[i]->GetVelocity();
right_speed = right_drive_[i]->GetVelocity();
}
const double lp = left_speed * step_time; // rad/s -> rad
const double rp = right_speed * step_time;
if (std::isnan(lp) || std::isnan(rp))
return;
left_pos += lp;
right_pos += rp;
}
left_pos /= wheel_joints_size_;
right_pos /= wheel_joints_size_;
left_wheel_curr_pos_ += left_pos;
right_wheel_curr_pos_ += right_pos;
// Estimate linear and angular velocity using joint information
odometry_.update(left_wheel_curr_pos_, right_wheel_curr_pos_, current_time);
if(enable_wheel_tf_)
{
if(left_wheel_tf_pub_->trylock())
{
geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(0, left_wheel_curr_pos_, 0);
for(auto &msg_tf : left_wheel_tf_pub_->msg_.transforms)
{
msg_tf.header.stamp = current_time;
msg_tf.transform.rotation = q;
}
left_wheel_tf_pub_->unlockAndPublish();
}
if(right_wheel_tf_pub_->trylock())
{
geometry_msgs::Quaternion q = tf::createQuaternionMsgFromRollPitchYaw(0, right_wheel_curr_pos_, 0);
for(auto &msg_tf : right_wheel_tf_pub_->msg_.transforms)
{
msg_tf.header.stamp = current_time;
msg_tf.transform.rotation = q;
}
right_wheel_tf_pub_->unlockAndPublish();
}
}
}
}
void ros_kinematics::RosDiffDrive::publishOdometry(const ros::Time& current_time)
{
// Publish odometry message
if (last_odom_state_publish_time_ + publish_period_ < current_time)
{
last_odom_state_publish_time_ += publish_period_;
// Compute and store orientation info
const geometry_msgs::Quaternion orientation(
tf::createQuaternionMsgFromYaw(odometry_.getHeading()));
// Populate odom message and publish
if (odom_pub_ && odom_pub_->trylock())
{
odom_pub_->msg_.header.stamp = current_time;
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_.getLinear();
odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
odom_pub_->unlockAndPublish();
}
// Publish tf /odom frame
if (enable_odom_tf_ && tf_odom_pub_->trylock())
{
geometry_msgs::TransformStamped& odom_frame = tf_odom_pub_->msg_.transforms[0];
odom_frame.header.stamp = current_time;
odom_frame.transform.translation.x = odometry_.getX();
odom_frame.transform.translation.y = odometry_.getY();
odom_frame.transform.rotation = orientation;
tf_odom_pub_->unlockAndPublish();
}
}
}
void ros_kinematics::RosDiffDrive::updateChild(void)
{
ros::Rate rate(update_rate_);
last_odom_state_publish_time_ = ros::Time::now();
last_odom_update_ = ros::Time::now();
odometry_.init(ros::Time::now());
while (ros::ok())
{
ros::Time current_time = ros::Time::now();
// update parameter from dynamic reconf
this->updateDynamicParams();
const double ws = wheel_separation_multiplier_ * wheel_separation_;
const double lwr = left_wheel_radius_multiplier_ * wheel_radius_;
const double rwr = right_wheel_radius_multiplier_ * wheel_radius_;
if (this->allReady())
{
this->updateOdometryEncoder(current_time);
this->publishOdometry(current_time);
}
// MOVE ROBOT
DriveCmd curr_cmd = *(command_.readFromRT());
const double dt = (current_time - curr_cmd.stamp).toSec();
// Brake if cmd_vel has timeout:
if (dt > cmd_vel_timeout_)
{
curr_cmd.linear.x = 0.0;
curr_cmd.angular.z = 0.0;
}
// Limit velocities and accelerations:
const double cmd_dt(dt);
limiter_lin_.limit(curr_cmd.linear.x, last0_cmd_.linear.x, last1_cmd_.linear.x, cmd_dt);
limiter_ang_.limit(curr_cmd.angular.z, last0_cmd_.angular.z, last1_cmd_.angular.z, cmd_dt);
last1_cmd_ = last0_cmd_;
last0_cmd_ = curr_cmd;
// Compute wheels velocities:
const double vel_left = (curr_cmd.linear.x - curr_cmd.angular.z * ws / 2.0) / lwr;
const double vel_right = (curr_cmd.linear.x + curr_cmd.angular.z * ws / 2.0) / rwr;
if(this->isCmdVelTriggered())
{
// Publish limited velocity:
if (publish_cmd_ && cmd_vel_pub_ && cmd_vel_pub_->trylock())
{
cmd_vel_pub_->msg_.header.stamp = current_time;
cmd_vel_pub_->msg_.twist.linear.x = curr_cmd.linear.x;
cmd_vel_pub_->msg_.twist.angular.z = curr_cmd.angular.z;
cmd_vel_pub_->unlockAndPublish();
}
for (size_t i = 0; i < wheel_joints_size_; ++i)
{
left_drive_[i]->SetVelocity(vel_left);
right_drive_[i]->SetVelocity(vel_right);
}
scheduleMotorController(false);
}
if(this->isCmdVelLastestTriggered())
{
int counter = 3;
do
{
for (size_t i = 0; i < wheel_joints_size_; ++i)
{
left_drive_[i]->SetVelocity(0.0);
right_drive_[i]->SetVelocity(0.0);
}
counter--;
rate.sleep();
ros::spinOnce();
}while(ros::ok() && counter > 0);
scheduleMotorController(false);
}
rate.sleep();
ros::spinOnce();
}
}
void ros_kinematics::RosDiffDrive::brake()
{
const double vel = 0.0;
for (size_t i = 0; i < wheel_joints_size_; ++i)
{
left_drive_[i]->SetVelocity(vel);
right_drive_[i]->SetVelocity(vel);
}
}
void ros_kinematics::RosDiffDrive::updateDynamicParams()
{
// Retreive dynamic params:
if (kinematics_param_ptr_ != nullptr)
{
left_wheel_radius_multiplier_ = kinematics_param_ptr_->getLeftWheelRadiusMultiplier();
right_wheel_radius_multiplier_ = kinematics_param_ptr_->getRightWheelRadiusMultiplier();
wheel_separation_multiplier_ = kinematics_param_ptr_->getWheelSeparationMultiplier();
publish_period_ = ros::Duration(1.0 / kinematics_param_ptr_->getPublishRate());
// enable_odom_tf_ = kinematics_param_ptr_->getEnableOdomTf();
}
}
void ros_kinematics::RosDiffDrive::setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh)
{
// Get and check params for covariances
XmlRpc::XmlRpcValue pose_cov_list;
controller_nh.getParam("pose_covariance_diagonal", pose_cov_list);
ROS_ASSERT(pose_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
ROS_ASSERT(pose_cov_list.size() == 6);
for (int i = 0; i < pose_cov_list.size(); ++i)
ROS_ASSERT(pose_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
XmlRpc::XmlRpcValue twist_cov_list;
controller_nh.getParam("twist_covariance_diagonal", twist_cov_list);
ROS_ASSERT(twist_cov_list.getType() == XmlRpc::XmlRpcValue::TypeArray);
ROS_ASSERT(twist_cov_list.size() == 6);
for (int i = 0; i < twist_cov_list.size(); ++i)
ROS_ASSERT(twist_cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
// Setup odometry realtime publisher + odom message constant fields
odom_pub_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(controller_nh, "odom", 100));
odom_pub_->msg_.header.frame_id = odom_frame_id_;
odom_pub_->msg_.child_frame_id = base_frame_id_;
odom_pub_->msg_.pose.pose.position.z = 0;
odom_pub_->msg_.pose.covariance = {
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 = {
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);
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_frame_id_;
}
bool ros_kinematics::RosDiffDrive::getWheelNames(ros::NodeHandle& _nh,
const std::string& wheel_param,
std::vector<std::string>& wheel_names)
{
XmlRpc::XmlRpcValue wheel_list;
if (!nh_priv_.getParam(wheel_param, wheel_list))
{
ROS_ERROR_STREAM_NAMED(name_,
"Couldn't retrieve wheel param '" << wheel_param << "'.");
return false;
}
if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeArray)
{
if (wheel_list.size() == 0)
{
ROS_ERROR_STREAM_NAMED(name_,
"Wheel param '" << wheel_param << "' is an empty list");
return false;
}
for (int i = 0; i < wheel_list.size(); ++i)
{
if (wheel_list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
{
ROS_ERROR_STREAM_NAMED(name_,
"Wheel param '" << wheel_param << "' #" << i <<
" isn't a string.");
return false;
}
}
wheel_names.resize(wheel_list.size());
for (int i = 0; i < wheel_list.size(); ++i)
{
wheel_names[i] = static_cast<std::string>(wheel_list[i]);
}
}
else if (wheel_list.getType() == XmlRpc::XmlRpcValue::TypeString)
{
wheel_names.push_back(wheel_list);
}
else
{
ROS_ERROR_STREAM_NAMED(name_,
"Wheel param '" << wheel_param <<
"' is neither a list of strings nor a string.");
return false;
}
return true;
}
bool ros_kinematics::RosDiffDrive::getWheelParams(ros::NodeHandle& controller_nh,
const std::vector<std::string>& wheel_names,
std::map<std::string, XmlRpc::XmlRpcValue>& drive_param_map)
{
for (int i = 0; i < wheel_names.size(); ++i)
{
XmlRpc::XmlRpcValue element_param;
if (!nh_priv_.getParam(wheel_names[i], element_param))
{
ROS_ERROR_STREAM_NAMED(name_,
"Couldn't retrieve wheel param '" << wheel_names[i] << "'.");
return false;
}
// check type of member
if(element_param.hasMember("lookup_name"))
{
if(element_param["lookup_name"].getType() != XmlRpc::XmlRpcValue::TypeString)
{
ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->lookup_name" << " isn't a string.");
return false;
}
}
if(element_param.hasMember("mesurement_topic"))
{
if(element_param["mesurement_topic"].getType() != XmlRpc::XmlRpcValue::TypeString)
{
ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->mesurement_topic" << " isn't a string.");
return false;
}
}
if(element_param.hasMember("frame_id"))
{
if(element_param["frame_id"].getType() != XmlRpc::XmlRpcValue::TypeString)
{
ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->frame_id" << " isn't a string.");
return false;
}
}
if(element_param.hasMember("wheel_topic"))
{
if(element_param["wheel_topic"].getType() != XmlRpc::XmlRpcValue::TypeString)
{
ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->wheel_topic" << " isn't a string.");
return false;
}
}
if(element_param.hasMember("max_publish_rate"))
{
if( element_param["max_publish_rate"].getType() != XmlRpc::XmlRpcValue::TypeDouble &&
element_param["max_publish_rate"].getType() != XmlRpc::XmlRpcValue::TypeInt
)
{
ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->max_publish_rate" << " isn't a double/int.");
return false;
}
}
if(element_param.hasMember("subscribe_queue_size"))
{
if( element_param["subscribe_queue_size"].getType() != XmlRpc::XmlRpcValue::TypeDouble &&
element_param["subscribe_queue_size"].getType() != XmlRpc::XmlRpcValue::TypeInt
)
{
ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->subscribe_queue_size" << " isn't a double/int.");
return false;
}
}
if(element_param.hasMember("command_timeout"))
{
if( element_param["command_timeout"].getType() != XmlRpc::XmlRpcValue::TypeDouble &&
element_param["command_timeout"].getType() != XmlRpc::XmlRpcValue::TypeInt
)
{
ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->command_timeout" << " isn't a double/int.");
return false;
}
}
if(!element_param.hasMember("origin"))
{
ROS_ERROR_STREAM_NAMED(name_,
"Couldn't retrieve wheel param '" << wheel_names[i] << "'->origin.");
return false;
}
else
{
if(element_param["origin"].getType() != XmlRpc::XmlRpcValue::TypeArray)
{
ROS_ERROR_STREAM_NAMED(name_, "'" << wheel_names[i] << "'->origin" << " isn't a arrays.");
return false;
}
else
{
if(element_param["origin"].size() < 6) // x y z roll pich yaw
{
ROS_ERROR_STREAM_NAMED(name_,
"Wheel param '" << wheel_names[i] << "'->origin is an error array");
return false;
}
else
{
for (int j = 0; j < element_param["origin"].size(); ++j)
{
if( element_param["origin"][j].getType() != XmlRpc::XmlRpcValue::TypeDouble &&
element_param["origin"][j].getType() != XmlRpc::XmlRpcValue::TypeInt
)
{
ROS_ERROR_STREAM_NAMED(name_,
"Wheel param '" << wheel_names[i] << "'->origin #" << j <<
" isn't a double/int.");
return false;
}
}
}
}
}
// Done
drive_param_map[wheel_names[i]] = element_param;
}
return true;
}
bool ros_kinematics::RosDiffDrive::allReady()
{
bool result = true;
for (size_t i = 0; i < wheel_joints_size_; ++i)
{
result = result && left_drive_[i]->Ready() && right_drive_[i]->Ready();
}
return result;
}

117
Devices/Packages/ros_kinematics/src/ros_diff_drive_parameters.cpp Executable file
View File

@@ -0,0 +1,117 @@
#include <ros_kinematics/ros_diff_drive_parameters.h>
#include <memory>
#include <string>
#include <map>
namespace ros_kinematics
{
KinematicDiffDriveParameters::KinematicDiffDriveParameters()
: left_wheel_radius_multiplier_(0.0),
right_wheel_radius_multiplier_(0.0),
wheel_separation_multiplier_(0.0),
publish_rate_(0.0),
enable_odom_tf_(false),
setup_(false),
dsrv_(nullptr)
{
}
KinematicDiffDriveParameters::KinematicDiffDriveParameters(const ros::NodeHandle& nh)
: left_wheel_radius_multiplier_(0.0),
right_wheel_radius_multiplier_(0.0),
wheel_separation_multiplier_(0.0),
publish_rate_(0.0),
enable_odom_tf_(false),
setup_(false),
dsrv_(nullptr)
{
initialize(nh);
}
void KinematicDiffDriveParameters::initialize(const ros::NodeHandle& nh)
{
if (!initialized_)
{
nh_ = ros::NodeHandle(nh);
// the rest of the initial values are loaded through the dynamic reconfigure mechanisms
dsrv_ = std::make_shared<dynamic_reconfigure::Server<DiffDriveParametersConfig> >(nh_);
dynamic_reconfigure::Server<DiffDriveParametersConfig>::CallbackType cb =
boost::bind(&KinematicDiffDriveParameters::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
int m_subscribe_queue_size = 1000;
kinematics_sub_ = nh_.subscribe<dynamic_reconfigure::Config>("/agv_dynparam/parameter_updates", m_subscribe_queue_size, &KinematicDiffDriveParameters::kinematicsCallback, this);
initialized_ = true;
}
}
void KinematicDiffDriveParameters::reconfigureCB(DiffDriveParametersConfig& config, uint32_t level)
{
boost::lock_guard<boost::mutex> schedule_lockguard(reconfig_mutex);
// ROS_WARN("reconfigureCB");
left_wheel_radius_multiplier_ = config.left_wheel_radius_multiplier;
right_wheel_radius_multiplier_ = config.right_wheel_radius_multiplier;
wheel_separation_multiplier_ = config.wheel_separation_multiplier;
publish_rate_ = config.publish_rate;
enable_odom_tf_ = config.enable_odom_tf;
// reconfig_mutex.unlock();
}
void KinematicDiffDriveParameters::kinematicsCallback(const dynamic_reconfigure::Config::ConstPtr& param)
{
boost::lock_guard<boost::mutex> schedule_lockguard(reconfig_mutex);
// ROS_WARN("dynamic_reconfigure");
if (param != nullptr)
{
std::map<std::string, double> param_m;
for (auto db : param->doubles) param_m[db.name] = db.value;
auto it = param_m.find("left_wheel_radius_multiplier");
if (it != param_m.end())
{
left_wheel_radius_multiplier_ = param_m["left_wheel_radius_multiplier"];
ROS_INFO("[ros_kinematics]-left_wheel_radius_multiplier %f", left_wheel_radius_multiplier_);
}
else ROS_WARN("[ros_kinematics]-left_wheel_radius_multiplier %f", left_wheel_radius_multiplier_);
it = param_m.find("right_wheel_radius_multiplier");
if (it != param_m.end())
{
right_wheel_radius_multiplier_ = param_m["right_wheel_radius_multiplier"];
ROS_INFO("[ros_kinematics]-right_wheel_radius_multiplier %f", right_wheel_radius_multiplier_);
}
else ROS_WARN("[ros_kinematics]-right_wheel_radius_multiplier %f", right_wheel_radius_multiplier_);
it = param_m.find("wheel_separation_multiplier");
if (it != param_m.end())
{
wheel_separation_multiplier_ = param_m["wheel_separation_multiplier"];
ROS_INFO("[ros_kinematics]-wheel_separation_multiplier %f", wheel_separation_multiplier_);
}
else ROS_WARN("[ros_kinematics]-wheel_separation_multiplier %f", wheel_separation_multiplier_);
it = param_m.find("publish_rate");
if (it != param_m.end())
{
publish_rate_ = param_m["publish_rate"];
ROS_INFO("[ros_kinematics]-publish_rate %f", publish_rate_);
}
else ROS_WARN("[ros_kinematics]-publish_rate %f", publish_rate_);
// it = param_m.find("enable_odom_tf");
// if (it != param_m.end())
// {
// enable_odom_tf_ = param_m["enable_odom_tf"];
// ROS_INFO("[ros_kinematics]-enable_odom_tf %x", enable_odom_tf_);
// }
// else ROS_WARN("[ros_kinematics]-enable_odom_tf %x", enable_odom_tf_);
param_m.clear();
setup_ = true;
}
else ROS_WARN("dynamic_reconfigure param is null");
// reconfig_mutex.unlock();
}
} // namespace ros_kinematics

33
Devices/Packages/ros_kinematics/src/ros_kinematics_node.cpp Executable file
View File

@@ -0,0 +1,33 @@
#include <ros/ros.h>
#include <boost/shared_ptr.hpp>
#include "ros_kinematics/base_drive.h"
#include "ros_kinematics/ros_steer_drive.h"
#include "ros_kinematics/ros_diff_drive.h"
int main(int argc, char **argv)
{
/* Khoi tao Node */
ros::init(argc, argv, "ros_kinematics");
ros::NodeHandle nh;
int type;
nh.param(ros::this_node::getName()+"/type", type, 1);
/* Constructors */
boost::shared_ptr<ros_kinematics::BaseDrive> ros_drive = nullptr;
switch (type)
{
case 1:
ros_drive = boost::make_shared<ros_kinematics::RosSteerDrive>(nh, ros::this_node::getName());
break;
case 2:
ros_drive = boost::make_shared<ros_kinematics::RosDiffDrive>(nh, ros::this_node::getName());
break;
default:
ros_drive = boost::make_shared<ros_kinematics::RosSteerDrive>(nh, ros::this_node::getName());
break;
}
ros::spin();
ros_drive = nullptr;
return 0;
}

447
Devices/Packages/ros_kinematics/src/ros_steer_drive.cpp Executable file
View File

@@ -0,0 +1,447 @@
#include "ros_kinematics/ros_steer_drive.h"
#include <std_msgs/UInt16.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>
ros_kinematics::RosSteerDrive::RosSteerDrive(ros::NodeHandle & nh, const std::string &name)
: nh_(nh),
motor_loader_("models", "models::BaseSteerDrive"),
encoder_loader_("models", "models::BaseAbsoluteEncoder"),
last_odom_update_(ros::Time(0)),
last_actuator_update_(ros::Time(0)),
publish_time_(ros::Time(0)),
publish_lastest_time_(ros::Time(0))
{
nh_priv_ = ros::NodeHandle(nh_, name);
ROS_INFO("RosSteerDrive get node name is %s", name.c_str());
bool init_odom_enc;
nh_priv_.param("initialize_odom_enc", init_odom_enc, true);
odom_enc_initialized_ = !init_odom_enc;
double schedule_delay, schedule_lastest_delay;
nh_priv_.param("commandTopic", command_topic_, std::string("cmd_vel"));
nh_priv_.param("publishOdomTF", publishOdomTF_, false);
nh_priv_.param("odometryTopic", odometry_topic_, std::string("odom"));
nh_priv_.param("odometryFrame", odometry_frame_, std::string("odom"));
nh_priv_.param("odometryEncTopic", odometry_enc_topic_, std::string("odom_enc"));
nh_priv_.param("odometryEncChildFrame", odometry_enc_child_frame_, std::string("odom_enc"));
nh_priv_.param("robotBaseFrame", robot_base_frame_, std::string("base_link"));
nh_priv_.param("steerChildFrame", steer_id_, std::string("steer_link"));
nh_priv_.param("subscribe_queue_size", m_subscribe_queue_size_, 1);
nh_priv_.param("max_update_rate", update_rate_, 100.0);
nh_priv_.param("schedule_delay", schedule_delay, 0.005);
nh_priv_.param("schedule_lastest_delay", schedule_lastest_delay, 0.5);
nh_priv_.param("odomEncSteeringAngleOffset",odom_enc_steering_angle_offset_, 0.);
nh_priv_.param("wheelAcceleration", wheel_acceleration_, 0.);
nh_priv_.param("steering_fix_wheel_distance_x", steering_fix_wheel_distance_x_, 0.68);
nh_priv_.param("steering_fix_wheel_distance_y", steering_fix_wheel_distance_y_, 0.);
std::string steer_drive;
nh_priv_.param("steer_drive", steer_drive, std::string("motor_wheel::SteerMotor"));
try
{
steer_motor_ = motor_loader_.createInstance(steer_drive);
steer_motor_->initialize(nh_, name + "/" + motor_loader_.getName(steer_drive));
ros::NodeHandle nh(nh_priv_, motor_loader_.getName(steer_drive));
nh.param("method", steer_motor_mode_, std::string(""));
}
catch(const pluginlib::PluginlibException &ex)
{
ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s", steer_drive.c_str(), ex.what());
exit(1);
}
std::string traction_drive;
nh_priv_.param("traction_drive", traction_drive, std::string("motor_wheel::TractionMotor"));
nh_priv_.param("wheel_diameter", wheel_diameter_, 0.210);
try
{
traction_motor_ = motor_loader_.createInstance(traction_drive);
traction_motor_->initialize(nh_, name + "/" + motor_loader_.getName(traction_drive));
ros::NodeHandle nh(nh_priv_, motor_loader_.getName(traction_drive));
nh.param("method", traction_motor_mode_, std::string(""));
}
catch(const pluginlib::PluginlibException &ex)
{
ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s", traction_drive.c_str(), ex.what());
exit(1);
}
nh_.param(name + "/use_encoder", use_abs_encoder, false);
if(use_abs_encoder)
{
std::string abs_encoder;
nh_priv_.param("absolute_encoder", abs_encoder, std::string("absolute_encoder::Measurements"));
try
{
absolute_encoder_ = encoder_loader_.createInstance(abs_encoder);
absolute_encoder_->initialize(nh, "AbsoluteEncoder");
}
catch(const pluginlib::PluginlibException &ex)
{
ROS_FATAL("Failed to create the %s model, are you sure it is properly registered and that the containing library is built? Exception: %s", abs_encoder.c_str(), ex.what());
exit(1);
}
}
transform_broadcaster_ = boost::shared_ptr<tf::TransformBroadcaster>(new tf::TransformBroadcaster());
odometry_publisher_ = nh_.advertise<nav_msgs::Odometry>(odometry_topic_, 1);
odometry_enc_publisher_ = nh_.advertise<nav_msgs::Odometry>(odometry_enc_topic_, 1);
cmd_vel_subscriber_ = nh_.subscribe(command_topic_, m_subscribe_queue_size_, &ros_kinematics::RosSteerDrive::CmdVelCallback, this);
cmd_vel_feedback_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel_feedback", 1);
cmd_vel_rb = nh_.advertise<geometry_msgs::Twist>("cmd_vel_rb", 1);
steer_angle_pub_ = nh_.advertise<std_msgs::Float32>("steer_angle", 1);
// Initialize update rate stuff
if (this->update_rate_ > 0.0)
this->update_period_ = 1.0 / this->update_rate_;
else
this->update_period_ = 0.0;
schedule_delay_ = ros::Duration(schedule_delay);
schedule_lastest_delay_ = ros::Duration(schedule_lastest_delay);
// Kinematics
kinematics_param_ptr = boost::make_shared<ros_kinematics::KinematicSteerDriveParameters>();
kinematics_param_ptr->initialize(nh_);
// Thread
callback_thread_ = new boost::thread(&ros_kinematics::RosSteerDrive::UpdateChild,this);
}
ros_kinematics::RosSteerDrive::~RosSteerDrive()
{
if (callback_thread_)
{
callback_thread_->join();
delete (callback_thread_);
callback_thread_ = 0;
}
}
void ros_kinematics::RosSteerDrive::CmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
{
boost::lock_guard<boost::mutex> schedule_lockguard(callback_mutex_);
command_struct_.linear.x = cmd_msg->linear.x;
command_struct_.angular.z = cmd_msg->angular.z;
scheduleMotorController(true);
}
/**
* @brief returns true, if publishing of a cmd_vel topic is publishing
*/
bool ros_kinematics::RosSteerDrive::isCmdVelTriggered(void)
{
boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
return !publish_time_.isZero() && ros::Time::now() > publish_time_;
}
/**
* @brief returns true, if publishing of a cmd_vel topic is publishing
*/
bool ros_kinematics::RosSteerDrive::isCmdVelLastestTriggered(void)
{
boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
return !publish_lastest_time_.isZero() && ros::Time::now() > publish_lastest_time_;
}
/**
* @brief schedules MotorController function
* @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed.
*/
void ros_kinematics::RosSteerDrive::scheduleMotorController(bool schedule)
{
boost::lock_guard<boost::mutex> schedule_lockguard(publish_mutex_);
if(schedule && publish_time_.isZero())
{
publish_time_ = ros::Time::now() + schedule_delay_;
publish_lastest_time_ = ros::Time::now() + schedule_lastest_delay_;
}
if(!schedule && !publish_time_.isZero())
{
publish_time_ = ros::Time(0);
// publish_lastest_time_ = ros::Time(0);
}
}
void ros_kinematics::RosSteerDrive::MotorController(double target_speed, double target_steering_speed, double dt)
{
// TODO add the accelerations etc. properly
double applied_speed = target_speed;
double applied_steering_speed = target_steering_speed;
if(traction_motor_->Ready() && steer_motor_->Ready())
{
double current_speed = traction_motor_->GetVelocity();
double current_steering_speed = steer_motor_->GetVelocity();
if(kinematics_param_ptr->isSetup())
{
wheel_acceleration_ = kinematics_param_ptr->getWheelAcceleration();
}
if (wheel_acceleration_ > 0)
{
// TODO
// applied_speed = ...;
// applied_steering_speed = ...;
}
if(traction_motor_mode_ == std::string("speed_mode"))
traction_motor_->SetVelocity(applied_speed);
if(steer_motor_mode_ == std::string("position_mode"))
steer_motor_->SetPosition(applied_steering_speed);
else if(steer_motor_mode_ == std::string("speed_mode"))
steer_motor_->SetVelocity(applied_steering_speed);
}
if(!traction_motor_->Ready())
ROS_WARN_THROTTLE_NAMED(2.0, "ros_kinematics", "traction_motor_ is not Ready.");
if(!steer_motor_->Ready())
ROS_WARN_THROTTLE_NAMED(2.0, "ros_kinematics", "steer_motor_ is not Ready.");
}
void ros_kinematics::RosSteerDrive::publishSteerJoint(double odom_alpha)
{
ros::Time current_time = ros::Time::now();
tf::Quaternion qt;
tf::Vector3 vt;
qt.setRPY(0, 0, odom_alpha);
if(kinematics_param_ptr->isSetup())
{
steering_fix_wheel_distance_x_ = kinematics_param_ptr->getSteeringFixWheelDistanceX();
steering_fix_wheel_distance_y_ = kinematics_param_ptr->getSteeringFixWheelDistanceY();
}
vt = tf::Vector3(steering_fix_wheel_distance_x_, steering_fix_wheel_distance_y_, 0);
tf::Transform base_to_steer_link(qt, vt);
transform_broadcaster_->sendTransform( tf::StampedTransform(base_to_steer_link, current_time,
robot_base_frame_, steer_id_));
}
void ros_kinematics::RosSteerDrive::UpdateOdometryEncoder()
{
ros::Time current_time = ros::Time::now();
double step_time = ros::Duration(current_time - last_odom_update_).toSec();
last_odom_update_ = current_time;
double odom_alpha;
if(use_abs_encoder)
odom_alpha = absolute_encoder_->Position();
else
odom_alpha = steer_motor_->Position();
if(kinematics_param_ptr->isSetup())
{
odom_enc_steering_angle_offset_= kinematics_param_ptr->getOdomEncSteeringAngleOffset();
// ROS_WARN("odom_enc_steering_angle_offset_ %f", odom_enc_steering_angle_offset_);
}
odom_alpha += odom_enc_steering_angle_offset_;
std_msgs::Float32 msg;
msg.data = odom_alpha/M_PI*180;
steer_angle_pub_.publish(msg);
publishSteerJoint(odom_alpha);
// ROS_WARN_STREAM("Odom alpha " << odom_alpha << " "<< odom_alpha / M_PI *180);
// Distance travelled drive wheel
double drive_dist = traction_motor_->GetVelocity() * (wheel_diameter_ / 2) * step_time;
double dd = 0.;
double da = 0.;
if (fabs(odom_alpha) < 0.000001) // Avoid dividing with a very small number...
{
dd = drive_dist;
da = 0.;
}
else
{
if(kinematics_param_ptr->isSetup())
{
steering_fix_wheel_distance_x_ = kinematics_param_ptr->getSteeringFixWheelDistanceX();
steering_fix_wheel_distance_y_ = kinematics_param_ptr->getSteeringFixWheelDistanceY();
}
double r_stear = steering_fix_wheel_distance_x_ / sin(odom_alpha);
double r_fix = r_stear * cos(odom_alpha) - steering_fix_wheel_distance_y_;
dd = r_fix / r_stear * drive_dist; // Adjust the actual forward movement (located between the fixed front wheels) based on the radius of the drive wheel).
da = drive_dist / r_stear;
}
// Update the current estimate
double dx = dd * cos(pose_encoder_.theta + da / 2.);
double dy = dd * sin(pose_encoder_.theta + da / 2.);
// Compute odometric pose
pose_encoder_.x += dx;
pose_encoder_.y += dy;
pose_encoder_.theta += da;
double w = da / step_time;
double v = dd / step_time;
geometry_msgs::Twist fb;
fb.linear.x = v;
fb.angular.z = w;
cmd_vel_feedback_.publish(fb);
tf::Quaternion qt;
tf::Vector3 vt;
qt.setRPY(0, 0, pose_encoder_.theta);
vt = tf::Vector3(pose_encoder_.x, pose_encoder_.y, 0);
odom_enc_.pose.pose.position.x = vt.x();
odom_enc_.pose.pose.position.y = vt.y();
odom_enc_.pose.pose.position.z = vt.z();
odom_enc_.pose.pose.orientation.x = qt.x();
odom_enc_.pose.pose.orientation.y = qt.y();
odom_enc_.pose.pose.orientation.z = qt.z();
odom_enc_.pose.pose.orientation.w = qt.w();
odom_enc_.twist.twist.angular.z = w;
odom_enc_.twist.twist.linear.x = v;
odom_enc_.twist.twist.linear.y = 0.0;
}
void ros_kinematics::RosSteerDrive::PublishOdometry(double step_time)
{
// getting data form encoder integration
odom_ = odom_enc_;
ros::Time current_time = ros::Time::now();
std::string odom_frame = odometry_frame_;
std::string base_footprint_frame = robot_base_frame_;
tf::Quaternion qt;
tf::Vector3 vt;
if(publishOdomTF_)
{
qt = tf::Quaternion(odom_.pose.pose.orientation.x, odom_.pose.pose.orientation.y, odom_.pose.pose.orientation.z, odom_.pose.pose.orientation.w);
vt = tf::Vector3(odom_.pose.pose.position.x, odom_.pose.pose.position.y, odom_.pose.pose.position.z);
tf::Transform base_footprint_to_odom(qt, vt);
transform_broadcaster_->sendTransform(
tf::StampedTransform(base_footprint_to_odom, current_time,
odom_frame, base_footprint_frame));
}
// set covariance - TODO, fix this(!)
odom_.pose.covariance[0] = 0.00001;
odom_.pose.covariance[7] = 0.00001;
odom_.pose.covariance[14] = 1000000000000.0;
odom_.pose.covariance[21] = 1000000000000.0;
odom_.pose.covariance[28] = 1000000000000.0;
odom_.pose.covariance[35] = 0.001;
// set header
odom_.header.stamp = current_time;
odom_.header.frame_id = odom_frame;
odom_.child_frame_id = base_footprint_frame;
odometry_publisher_.publish(odom_);
// publish the encoder based odometry
{
if (!odom_enc_initialized_)
{
pose_encoder_.x = odom_.pose.pose.position.x;
pose_encoder_.y = odom_.pose.pose.position.y;
pose_encoder_.theta = tf::getYaw(odom_.pose.pose.orientation);
odom_enc_initialized_ = true;
odom_enc_ = odom_;
}
std::string odom_enc_child_frame = odometry_enc_child_frame_;
if(publishOdomTF_)
{
qt = tf::Quaternion(odom_enc_.pose.pose.orientation.x, odom_enc_.pose.pose.orientation.y, odom_enc_.pose.pose.orientation.z, odom_enc_.pose.pose.orientation.w);
vt = tf::Vector3(odom_enc_.pose.pose.position.x, odom_enc_.pose.pose.position.y, odom_enc_.pose.pose.position.z);
tf::Transform odom_enc_child_to_odom(qt, vt);
transform_broadcaster_->sendTransform(
tf::StampedTransform(odom_enc_child_to_odom, current_time,
odom_frame, odom_enc_child_frame));
}
odom_enc_.pose.covariance = odom_.pose.covariance; // TODO...
odom_enc_.header.stamp = current_time;
odom_enc_.header.frame_id = odom_frame; // Note - this is typically /world
odom_enc_.child_frame_id = odom_enc_child_frame;
odometry_enc_publisher_.publish(odom_enc_);
}
}
void ros_kinematics::RosSteerDrive::UpdateChild(void)
{
ros::Rate rate(update_rate_);
bool stopped;
while (ros::ok())
{
if(traction_motor_->Ready() && steer_motor_->Ready())
{
UpdateOdometryEncoder();
}
ros::Time current_time = ros::Time::now();
double seconds_since_last_update = ros::Duration(current_time - last_actuator_update_).toSec();
if (seconds_since_last_update > update_period_)
{
if(traction_motor_->Ready() && steer_motor_->Ready())
{
PublishOdometry(seconds_since_last_update);
}
// if (publishWheelTF_)
// publishWheelTF();
// if (publishWheelJointState_)
// publishWheelJointState();
double target_steering_angle_speed_saved_;
if(isCmdVelTriggered())
{
double target_wheel_rotation_speed = command_struct_.linear.x / (wheel_diameter_ / 2.0);
double target_steering_angle_speed = command_struct_.angular.z;
double odom_alpha;
if(use_abs_encoder)
odom_alpha = absolute_encoder_->Position();
else
odom_alpha = steer_motor_->Position();
if(kinematics_param_ptr->isSetup())
{
odom_enc_steering_angle_offset_= kinematics_param_ptr->getOdomEncSteeringAngleOffset();
}
odom_alpha += odom_enc_steering_angle_offset_;
geometry_msgs::Twist fb;
fb.linear.x = command_struct_.linear.x * fabs(cos(odom_alpha));
cmd_vel_rb.publish(fb);
MotorController(target_wheel_rotation_speed, target_steering_angle_speed, seconds_since_last_update);
target_steering_angle_speed_saved_ = target_steering_angle_speed;
scheduleMotorController(false);
stopped = false;
}
if(isCmdVelLastestTriggered() && !stopped)
{
if(steer_motor_mode_ == std::string("position_mode"))
{
MotorController(0, target_steering_angle_speed_saved_, seconds_since_last_update);
MotorController(0, target_steering_angle_speed_saved_, seconds_since_last_update);
}
else
{
MotorController(0, 0, seconds_since_last_update);
MotorController(0, 0, seconds_since_last_update);
}
stopped = true;
}
last_actuator_update_ = current_time;
}
rate.sleep();
ros::spinOnce();
}
}

106
Devices/Packages/ros_kinematics/src/ros_steer_drive_parameters.cpp Executable file
View File

@@ -0,0 +1,106 @@
#include <ros_kinematics/ros_steer_drive_parameters.h>
#include <memory>
#include <string>
#include <map>
namespace ros_kinematics
{
KinematicSteerDriveParameters::KinematicSteerDriveParameters()
: odom_enc_steering_angle_offset_(0.0),
steering_fix_wheel_distance_x_(0.0),
steering_fix_wheel_distance_y_(0.0),
wheel_acceleration_(0.0),
setup_(false),
dsrv_(nullptr)
{
}
KinematicSteerDriveParameters::KinematicSteerDriveParameters(const ros::NodeHandle& nh)
: odom_enc_steering_angle_offset_(0.0),
steering_fix_wheel_distance_x_(0.0),
steering_fix_wheel_distance_y_(0.0),
wheel_acceleration_(0.0),
setup_(false),
dsrv_(nullptr)
{
initialize(nh);
}
void KinematicSteerDriveParameters::initialize(const ros::NodeHandle& nh)
{
if (!initialized_)
{
nh_ = ros::NodeHandle(nh);
// the rest of the initial values are loaded through the dynamic reconfigure mechanisms
dsrv_ = std::make_shared<dynamic_reconfigure::Server<SteerDriveParametersConfig> >(nh_);
dynamic_reconfigure::Server<SteerDriveParametersConfig>::CallbackType cb =
boost::bind(&KinematicSteerDriveParameters::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
int m_subscribe_queue_size = 1000;
kinematics_sub_ = nh_.subscribe<dynamic_reconfigure::Config>("/agv_dynparam/parameter_updates", m_subscribe_queue_size, &KinematicSteerDriveParameters::kinematicsCallback, this);
initialized_ = true;
}
}
void KinematicSteerDriveParameters::reconfigureCB(SteerDriveParametersConfig& config, uint32_t level)
{
boost::lock_guard<boost::mutex> schedule_lockguard(reconfig_mutex);
// ROS_WARN("reconfigureCB");
odom_enc_steering_angle_offset_ = config.odomEncSteeringAngleOffset;
steering_fix_wheel_distance_x_ = config.steering_fix_wheel_distance_x;
steering_fix_wheel_distance_y_ = config.steering_fix_wheel_distance_y;
wheel_acceleration_ = config.wheelAcceleration;
// reconfig_mutex.unlock();
}
void KinematicSteerDriveParameters::kinematicsCallback(const dynamic_reconfigure::Config::ConstPtr& param)
{
boost::lock_guard<boost::mutex> schedule_lockguard(reconfig_mutex);
// ROS_WARN("dynamic_reconfigure");
if (param != nullptr)
{
std::map<std::string, double> param_m;
for (auto db : param->doubles) param_m[db.name] = db.value;
auto it = param_m.find("odomEncSteeringAngleOffset");
if (it != param_m.end())
{
odom_enc_steering_angle_offset_ = param_m["odomEncSteeringAngleOffset"];
ROS_INFO("[ros_kinematics]-odomEncSteeringAngleOffset %f", odom_enc_steering_angle_offset_);
}
else ROS_WARN("[ros_kinematics]-odomEncSteeringAngleOffset %f", odom_enc_steering_angle_offset_);
it = param_m.find("steering_fix_wheel_distance_x");
if (it != param_m.end())
{
steering_fix_wheel_distance_x_ = param_m["steering_fix_wheel_distance_x"];
ROS_INFO("[ros_kinematics]-steering_fix_wheel_distance_x %f", steering_fix_wheel_distance_x_);
}
else ROS_WARN("[ros_kinematics]-steering_fix_wheel_distance_x %f", steering_fix_wheel_distance_x_);
it = param_m.find("steering_fix_wheel_distance_y");
if (it != param_m.end())
{
steering_fix_wheel_distance_y_ = param_m["steering_fix_wheel_distance_y"];
ROS_INFO("[ros_kinematics]-steering_fix_wheel_distance_y %f", steering_fix_wheel_distance_y_);
}
else ROS_WARN("[ros_kinematics]-steering_fix_wheel_distance_y %f", steering_fix_wheel_distance_y_);
it = param_m.find("wheelAcceleration");
if (it != param_m.end())
{
wheel_acceleration_ = param_m["wheelAcceleration"];
ROS_INFO("[ros_kinematics]-wheelAcceleration %f", wheel_acceleration_);
}
else ROS_WARN("[ros_kinematics]-wheelAcceleration %f", wheel_acceleration_);
param_m.clear();
setup_ = true;
}
else ROS_WARN("dynamic_reconfigure param is null");
// reconfig_mutex.unlock();
}
} // namespace ros_kinematics

137
Devices/Packages/ros_kinematics/src/speed_limiter.cpp Executable file
View File

@@ -0,0 +1,137 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2013, PAL Robotics, S.L.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the PAL Robotics nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/*
* Author: Enrique Fernández
*/
#include <algorithm>
#include <ros_kinematics/speed_limiter.h>
template<typename T>
T clamp(T x, T min, T max)
{
return std::min(std::max(min, x), max);
}
namespace ros_kinematics
{
SpeedLimiter::SpeedLimiter(
bool has_velocity_limits,
bool has_acceleration_limits,
bool has_jerk_limits,
double min_velocity,
double max_velocity,
double min_acceleration,
double max_acceleration,
double min_jerk,
double max_jerk
)
: has_velocity_limits(has_velocity_limits)
, has_acceleration_limits(has_acceleration_limits)
, has_jerk_limits(has_jerk_limits)
, min_velocity(min_velocity)
, max_velocity(max_velocity)
, min_acceleration(min_acceleration)
, max_acceleration(max_acceleration)
, min_jerk(min_jerk)
, max_jerk(max_jerk)
{
}
double SpeedLimiter::limit(double& v, double v0, double v1, double dt)
{
const double tmp = v;
limit_jerk(v, v0, v1, dt);
limit_acceleration(v, v0, dt);
limit_velocity(v);
return tmp != 0.0 ? v / tmp : 1.0;
}
double SpeedLimiter::limit_velocity(double& v)
{
const double tmp = v;
if (has_velocity_limits)
{
v = clamp(v, min_velocity, max_velocity);
}
return tmp != 0.0 ? v / tmp : 1.0;
}
double SpeedLimiter::limit_acceleration(double& v, double v0, double dt)
{
const double tmp = v;
if (has_acceleration_limits)
{
const double dv_min = min_acceleration * dt;
const double dv_max = max_acceleration * dt;
const double dv = clamp(v - v0, dv_min, dv_max);
v = v0 + dv;
}
return tmp != 0.0 ? v / tmp : 1.0;
}
double SpeedLimiter::limit_jerk(double& v, double v0, double v1, double dt)
{
const double tmp = v;
if (has_jerk_limits)
{
const double dv = v - v0;
const double dv0 = v0 - v1;
const double dt2 = 2. * dt * dt;
const double da_min = min_jerk * dt2;
const double da_max = max_jerk * dt2;
const double da = clamp(dv - dv0, da_min, da_max);
v = v0 + dv0 + da;
}
return tmp != 0.0 ? v / tmp : 1.0;
}
} // namespace ros_kinematics