Skip to content

Gyro controls #17

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: indigo-devel
Choose a base branch
from
Open

Gyro controls #17

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 9 additions & 2 deletions robot_controllers/include/robot_controllers/diff_drive_base.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,7 @@
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Imu.h>

namespace robot_controllers
{
Expand Down Expand Up @@ -121,6 +122,9 @@ class DiffDriveBaseController : public Controller
/** @brief Command callback from either a ROS topic, or a higher controller. */
void command(const geometry_msgs::TwistConstPtr& msg);

/** @brief Imu callback .*/
void imuCallback(const sensor_msgs::ImuConstPtr& msg);

private:
bool initialized_;
ControllerManager* manager_;
Expand All @@ -129,7 +133,7 @@ class DiffDriveBaseController : public Controller
void scanCallback(const sensor_msgs::LaserScanConstPtr& scan);

// Set base wheel speeds in m/s
void setCommand(float left, float right);
void setCommand(float left, float right, double effort_l_, double effort_r_);

JointHandlePtr left_;
JointHandlePtr right_;
Expand All @@ -146,6 +150,8 @@ class DiffDriveBaseController : public Controller
double max_velocity_r_;
double max_acceleration_x_;
double max_acceleration_r_;
double gain_effort_;
double gain_velocity_;

// Laser can provide additional safety limits on velocity
double safety_scaling_;
Expand All @@ -161,6 +167,7 @@ class DiffDriveBaseController : public Controller
// These are from controller update
float last_sent_x_;
float last_sent_r_;
float gyro_z_;

float left_last_position_;
float right_last_position_;
Expand All @@ -175,7 +182,7 @@ class DiffDriveBaseController : public Controller
nav_msgs::Odometry odom_;
ros::Publisher odom_pub_;
ros::Timer odom_timer_;
ros::Subscriber cmd_sub_, scan_sub_;
ros::Subscriber cmd_sub_, scan_sub_, imu_sub_;

boost::shared_ptr<tf::TransformBroadcaster> broadcaster_;
bool publish_tf_;
Expand Down
46 changes: 38 additions & 8 deletions robot_controllers/src/diff_drive_base.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -108,6 +108,12 @@ int DiffDriveBaseController::init(ros::NodeHandle& nh, ControllerManager* manage
nh.param<double>("rotating_threshold", rotating_threshold_, 0.05);
nh.param<double>("moving_threshold", moving_threshold_, 0.05);

nh.param<double>("gain_effort",gain_effort_, 0.0);
ROS_INFO("The gyro gain on the effort is %f", gain_effort_);

nh.param<double>("gain_velocity", gain_velocity_, 0.0);
ROS_INFO("The gyro gain on the velocity is %f", gain_velocity_);

double t;
nh.param<double>("timeout", t, 0.25);
timeout_ = ros::Duration(t);
Expand All @@ -122,6 +128,10 @@ int DiffDriveBaseController::init(ros::NodeHandle& nh, ControllerManager* manage
cmd_sub_ = nh.subscribe<geometry_msgs::Twist>("command", 1,
boost::bind(&DiffDriveBaseController::command, this, _1));


// Subscribe to imu
imu_sub_ = nh.subscribe<sensor_msgs::Imu>("/imu",1, &DiffDriveBaseController::imuCallback, this);

// Publish odometry & tf
ros::NodeHandle n;
odom_pub_ = n.advertise<nav_msgs::Odometry>("odom", 10);
Expand Down Expand Up @@ -172,6 +182,13 @@ void DiffDriveBaseController::command(const geometry_msgs::TwistConstPtr& msg)
manager_->requestStart(getName());
}

// callback function for imu
void DiffDriveBaseController::imuCallback(const sensor_msgs::ImuConstPtr & msg)
{
boost::mutex::scoped_lock lock(command_mutex_);
gyro_z_ = msg->angular_velocity.z;
}

bool DiffDriveBaseController::start()
{
if (!initialized_)
Expand Down Expand Up @@ -227,8 +244,8 @@ void DiffDriveBaseController::update(const ros::Time& now, const ros::Duration&
safety_scaling_ = 0.1;
}

// Do velocity acceleration/limiting
double x, r;
// Do velocity acceleration/limiting, get gyro_z with holding lock
double x, r, gyro_z;
{
boost::mutex::scoped_lock lock(command_mutex_);
// Limit linear velocity based on obstacles
Expand All @@ -240,6 +257,7 @@ void DiffDriveBaseController::update(const ros::Time& now, const ros::Duration&
// Limit angular velocity
// Scale same amount as linear velocity so that robot still follows the same "curvature"
r = std::max(-max_velocity_r_, std::min(actual_scaling * desired_r_, max_velocity_r_));
gyro_z = gyro_z_;
}
if (x > last_sent_x_)
{
Expand Down Expand Up @@ -268,13 +286,16 @@ void DiffDriveBaseController::update(const ros::Time& now, const ros::Duration&

double dx = 0.0;
double dr = 0.0;

double left_pos = left_->getPosition();
double right_pos = right_->getPosition();
double left_dx = angles::shortest_angular_distance(left_last_position_, left_pos)/radians_per_meter_;
double right_dx = angles::shortest_angular_distance(right_last_position_, right_pos)/radians_per_meter_;
double left_vel = static_cast<double>(left_->getVelocity())/radians_per_meter_;
double right_vel = static_cast<double>(right_->getVelocity())/radians_per_meter_;
double effort_l;
double effort_r;
double ang_err;
double p_gain_effort, p_gain_velocity;

// Threshold the odometry to avoid noise (especially in simulation)
if (fabs(left_dx) > wheel_rotating_threshold_ ||
Expand All @@ -301,14 +322,23 @@ void DiffDriveBaseController::update(const ros::Time& now, const ros::Duration&
dx = (left_vel + right_vel)/2.0;
dr = (right_vel - left_vel)/track_width_;

// add torque along with velocity for stability
ang_err = dr - gyro_z;
p_gain_effort = gain_effort_ * ang_err;
effort_r = p_gain_effort;
effort_l = -p_gain_effort;
p_gain_velocity = gain_velocity_ * ang_err;

// Actually set command
if (fabs(dx) > moving_threshold_ ||
fabs(dr) > rotating_threshold_ ||
last_sent_x_ != 0.0 ||
last_sent_r_ != 0.0)
{
setCommand(last_sent_x_ - (last_sent_r_/2.0 * track_width_),
last_sent_x_ + (last_sent_r_/2.0 * track_width_));
double vel_r = last_sent_x_ - ((last_sent_r_ + p_gain_velocity)/2.0 * track_width_);
double vel_l = last_sent_x_ + ((last_sent_r_ + p_gain_velocity)/2.0 * track_width_);

setCommand(vel_r, vel_l, effort_l, effort_r);
}

// Lock mutex before updating
Expand Down Expand Up @@ -401,11 +431,11 @@ void DiffDriveBaseController::scanCallback(
last_laser_scan_ = ros::Time::now();
}

void DiffDriveBaseController::setCommand(float left, float right)
void DiffDriveBaseController::setCommand(float left, float right, double effort_l_, double effort_r_)
{
// Convert meters/sec into radians/sec
left_->setVelocity(left * radians_per_meter_, 0.0);
right_->setVelocity(right * radians_per_meter_, 0.0);
left_->setVelocity(left * radians_per_meter_, effort_l_);
right_->setVelocity(right * radians_per_meter_, effort_r_);
}

} // namespace robot_controllers