bool DiffDriveController::init(hardware_interface::VelocityJointInterface* hw,
ros::NodeHandle& root_nh,
ros::NodeHandle &controller_nh)
{
const std::string complete_ns = controller_nh.getNamespace();
std::size_t id = complete_ns.find_last_of("/");
name_ = complete_ns.substr(id + 1);
// Get joint names from the parameter server
std::vector<std::string> left_wheel_names, right_wheel_names;
if (!getWheelNames(controller_nh, "left_wheel", left_wheel_names) or
!getWheelNames(controller_nh, "right_wheel", right_wheel_names))
{
return false;
}
if (left_wheel_names.size() != right_wheel_names.size())
{
ROS_ERROR_STREAM_NAMED(name_,
"#left wheels (" << left_wheel_names.size() << ") != " <<
"#right wheels (" << right_wheel_names.size() << ").");
return false;
}
else
{
wheel_joints_size_ = left_wheel_names.size();
left_wheel_joints_.resize(wheel_joints_size_);
right_wheel_joints_.resize(wheel_joints_size_);
}
// 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_ << ".");
controller_nh.param("wheel_radius_multiplier", wheel_radius_multiplier_, wheel_radius_multiplier_);
ROS_INFO_STREAM_NAMED(name_, "Wheel radius will be multiplied by "
<< wheel_radius_multiplier_ << ".");
int velocity_rolling_window_size = 10;
controller_nh.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:
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.");
controller_nh.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"));
controller_nh.param("base_frame_id", base_frame_id_, base_frame_id_);
ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_frame_id_);
controller_nh.param("odom_frame_id", odom_frame_id_, odom_frame_id_);
ROS_INFO_STREAM_NAMED(name_, "Odometry frame_id set to " << odom_frame_id_);
controller_nh.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_);
ROS_INFO_STREAM_NAMED(name_, "Publishing to tf is " << (enable_odom_tf_?"enabled":"disabled"));
// Velocity and acceleration limits:
controller_nh.param("linear/x/has_velocity_limits" , limiter_lin_.has_velocity_limits , limiter_lin_.has_velocity_limits );
controller_nh.param("linear/x/has_acceleration_limits", limiter_lin_.has_acceleration_limits, limiter_lin_.has_acceleration_limits);
controller_nh.param("linear/x/has_jerk_limits" , limiter_lin_.has_jerk_limits , limiter_lin_.has_jerk_limits );
controller_nh.param("linear/x/max_velocity" , limiter_lin_.max_velocity , limiter_lin_.max_velocity );
controller_nh.param("linear/x/min_velocity" , limiter_lin_.min_velocity , -limiter_lin_.max_velocity );
controller_nh.param("linear/x/max_acceleration" , limiter_lin_.max_acceleration , limiter_lin_.max_acceleration );
controller_nh.param("linear/x/min_acceleration" , limiter_lin_.min_acceleration , -limiter_lin_.max_acceleration );
controller_nh.param("linear/x/max_jerk" , limiter_lin_.max_jerk , limiter_lin_.max_jerk );
controller_nh.param("linear/x/min_jerk" , limiter_lin_.min_jerk , -limiter_lin_.max_jerk );
controller_nh.param("angular/z/has_velocity_limits" , limiter_ang_.has_velocity_limits , limiter_ang_.has_velocity_limits );
controller_nh.param("angular/z/has_acceleration_limits", limiter_ang_.has_acceleration_limits, limiter_ang_.has_acceleration_limits);
controller_nh.param("angular/z/has_jerk_limits" , limiter_ang_.has_jerk_limits , limiter_ang_.has_jerk_limits );
controller_nh.param("angular/z/max_velocity" , limiter_ang_.max_velocity , limiter_ang_.max_velocity );
controller_nh.param("angular/z/min_velocity" , limiter_ang_.min_velocity , -limiter_ang_.max_velocity );
controller_nh.param("angular/z/max_acceleration" , limiter_ang_.max_acceleration , limiter_ang_.max_acceleration );
controller_nh.param("angular/z/min_acceleration" , limiter_ang_.min_acceleration , -limiter_ang_.max_acceleration );
controller_nh.param("angular/z/max_jerk" , limiter_ang_.max_jerk , limiter_ang_.max_jerk );
controller_nh.param("angular/z/min_jerk" , limiter_ang_.min_jerk , -limiter_ang_.max_jerk );
// If either parameter is not available, we need to look up the value in the URDF
bool lookup_wheel_separation = !controller_nh.getParam("wheel_separation", wheel_separation_);
bool lookup_wheel_radius = !controller_nh.getParam("wheel_radius", wheel_radius_);
if (!setOdomParamsFromUrdf(root_nh,
left_wheel_names[0],
right_wheel_names[0],
lookup_wheel_separation,
lookup_wheel_radius))
{
return false;
}
// 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 wr = wheel_radius_multiplier_ * wheel_radius_;
odometry_.setWheelParams(ws, wr);
ROS_INFO_STREAM_NAMED(name_,
"Odometry params : wheel separation " << ws
<< ", wheel radius " << wr);
setOdomPubFields(root_nh, controller_nh);
// Get the joint object to use in the realtime loop
for (int i = 0; i < wheel_joints_size_; ++i)
{
ROS_INFO_STREAM_NAMED(name_,
"Adding left wheel with joint name: " << left_wheel_names[i]
<< " and right wheel with joint name: " << right_wheel_names[i]);
left_wheel_joints_[i] = hw->getHandle(left_wheel_names[i]); // throws on failure
right_wheel_joints_[i] = hw->getHandle(right_wheel_names[i]); // throws on failure
}
sub_command_ = controller_nh.subscribe("cmd_vel", 1, &DiffDriveController::cmdVelCallback, this);
return true;
}
bool OmniDriveController::init(hardware_interface::VelocityJointInterface* hw,
ros::NodeHandle& root_nh,
ros::NodeHandle &controller_nh)
{
const std::string complete_ns = controller_nh.getNamespace();
std::size_t id = complete_ns.find_last_of("/");
name_ = complete_ns.substr(id + 1);
// Get joint names from the parameter server
if (!getWheelNames(controller_nh, "wheel1", wheel1_name) ||
!getWheelNames(controller_nh, "wheel2", wheel2_name) ||
!getWheelNames(controller_nh, "wheel3", wheel3_name))
{
return false;
}
// Odometry related:
double publish_rate;
controller_nh.param("publish_rate", publish_rate, 20.0);
ROS_INFO_STREAM_NAMED(name_, "Controller state will be published at "
<< publish_rate << "Hz.");
publish_period_ = ros::Duration(1.0 / publish_rate);
// 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.");
controller_nh.param("base_frame_id", base_frame_id_, base_frame_id_);
ROS_INFO_STREAM_NAMED(name_, "Base frame_id set to " << base_frame_id_);
controller_nh.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_);
ROS_INFO_STREAM_NAMED(name_, "Publishing to tf is " << (enable_odom_tf_?"enabled":"disabled"));
// Velocity and acceleration limits:
controller_nh.param("linear/x/has_velocity_limits" , limiter_linear_x_.has_velocity_limits , limiter_linear_x_.has_velocity_limits );
controller_nh.param("linear/x/has_acceleration_limits", limiter_linear_x_.has_acceleration_limits, limiter_linear_x_.has_acceleration_limits);
controller_nh.param("linear/x/max_velocity" , limiter_linear_x_.max_velocity , limiter_linear_x_.max_velocity );
controller_nh.param("linear/x/min_velocity" , limiter_linear_x_.min_velocity , -limiter_linear_x_.max_velocity );
controller_nh.param("linear/x/max_acceleration" , limiter_linear_x_.max_acceleration , limiter_linear_x_.max_acceleration );
controller_nh.param("linear/x/min_acceleration" , limiter_linear_x_.min_acceleration , -limiter_linear_x_.max_acceleration );
controller_nh.param("linear/y/has_velocity_limits" , limiter_linear_y_.has_velocity_limits , limiter_linear_y_.has_velocity_limits );
controller_nh.param("linear/y/has_acceleration_limits", limiter_linear_y_.has_acceleration_limits, limiter_linear_y_.has_acceleration_limits);
controller_nh.param("linear/y/max_velocity" , limiter_linear_y_.max_velocity , limiter_linear_y_.max_velocity );
controller_nh.param("linear/y/min_velocity" , limiter_linear_y_.min_velocity , -limiter_linear_y_.max_velocity );
controller_nh.param("linear/y/max_acceleration" , limiter_linear_y_.max_acceleration , limiter_linear_y_.max_acceleration );
controller_nh.param("linear/y/min_acceleration" , limiter_linear_y_.min_acceleration , -limiter_linear_y_.max_acceleration );
controller_nh.param("angular/z/has_velocity_limits" , limiter_angular_.has_velocity_limits , limiter_angular_.has_velocity_limits );
controller_nh.param("angular/z/has_acceleration_limits", limiter_angular_.has_acceleration_limits, limiter_angular_.has_acceleration_limits);
controller_nh.param("angular/z/max_velocity" , limiter_angular_.max_velocity , limiter_angular_.max_velocity );
controller_nh.param("angular/z/min_velocity" , limiter_angular_.min_velocity , -limiter_angular_.max_velocity );
controller_nh.param("angular/z/max_acceleration" , limiter_angular_.max_acceleration , limiter_angular_.max_acceleration );
controller_nh.param("angular/z/min_acceleration" , limiter_angular_.min_acceleration , -limiter_angular_.max_acceleration );
if (!setOdomParamsFromUrdf(root_nh, wheel1_name, wheel2_name, wheel3_name))
return false;
setOdomPubFields(root_nh, controller_nh);
// Get the joint object to use in the realtime loop
wheel1_joint = hw->getHandle(wheel1_name); // throws on failure
wheel2_joint = hw->getHandle(wheel2_name); // throws on failure
wheel3_joint = hw->getHandle(wheel3_name);
//test
sub_command_ = controller_nh.subscribe("smooth_cmd_vel", 1, &OmniDriveController::cmdVelCallback, this);
//sub_command_ = controller_nh.subscribe("cmd_vel", 1, &OmniDriveController::cmdVelCallback, this);
// ROS_INFO("wocao");
return true;
}
