Turtlebot 2 with Kobuki base - Jerky movement and Warning message

edit

I am running the turtlebot 2 with c++ code I wrote. I continuously get an error message stating:

Is robot stalled? Motors current is very high

After the predetermined halt message I input (5 second delay) the turtlebot will continue moving again, and then halt after another second or so. Is there anything that I can do?

Here is how I am publishing velocity commands:

 #include "ros/ros.h"
//The following have the < and > symbols removed so that they appear in ros answers.
 #include iostream
 #include string
 #include vector
 #include math.h
 #include stdlib.h
 #include time.h
 #include diagnostic_msgs/DiagnosticStatus.h
 #include diagnostic_msgs/DiagnosticArray.h
 #include sensor_msgs/LaserScan.h
 #include geometry_msgs/Twist.h
 #include std_msgs/Int32.h
 using namespace std; 

 double secondsLaptop = 0;
 double secondsCreate = 0;

 //abort flag turned on if bump or wheeldrop sensors activated, or warning or error  messages are thrown
 bool ab0rt = 0;

 bool rotateDirSet=0;

 //velocities 
 double linearVel = 0.0;
 double angularVel = 0.0; 

 //vector containing laserscan readings within kinect's field of view
 std::vector<double> rangeReadings;

 //these hold average distance values for leftmost and rightmost laser readings
 double distToLeftWall;
 double distToRightWall;
 double centerLeft;
 double centerRight;

//flag turned on when vector containing laserscan readings is initalized
bool rangesInit=0;

void laserAggCallback(const sensor_msgs::LaserScan::ConstPtr &msg)
{
  //sensor_msgs::LaserScan points = msg->ranges[i];

  //purge vector immediately before fresh values are pushed on
  rangeReadings.clear();

  //fill vector with full range of values from laserScan message (320 degrees)
  for(int i=0; i<640; i++)
  {
    rangeReadings.push_back(msg->ranges[i]);
    int k = i-1;
    if(i==0 && rangeReadings[i] != rangeReadings[i])
    {
      rangeReadings[i] = 0.0;
      //cout << rangeReadings[i] << endl;
    }

else if(rangeReadings[i] != rangeReadings[i])
{
  rangeReadings[i] = rangeReadings[k];
 // cout << rangeReadings[i] << endl;
}
 
// ROS_INFO("Range[%d]: [%f]",i ,rangeReadings[i]);
  }
  rangesInit=1;
}
 
/* This method checks for nearby obstacles/walls and 
 * allows the robot to stay centered while moving down a hallway. 
 */
void driveCentered()
{
  if (!rangeReadings.empty())
  {
 
//variables to store average far left and far right laser scan readings
distToLeftWall=0;
distToRightWall=0;
centerLeft=0;
centerRight=0;

// holds the minimum laser scan reading
double minDistance=11.0;
// holds the index of the minimum laser scan reading
int minIndex=320;
// holds the average of all laser scan readings within Kinect's FOV
double avgDist=0;

//extract only the laser scan readings that are within the Kinect's field of view
for (int i=6;i<640;i++)
{
  avgDist += rangeReadings[i];
  if (rangeReadings[i]<minDistance)
  {
   minIndex = i;
   minDistance=rangeReadings[i];
  }
}  

avgDist /= 634;


// check for objects/wall closer than 1m or for high avg distance reading indicating that
// the robot is directly facing a very close wall or corner
//if (minDistance < 1 || avgDist >= 9.0)
if(avgDist <= 0.6)
{

 // ROS_INFO("minDistance: %f", minDistance);
 // ROS_INFO("avgDist: %f", avgDist);
  // do not move forward
  linearVel = 0.0;
  //Make waiter turtlebot wait for 5 seconds *****
  ros::Duration(5).sleep();
  // choose rotation direction based on location of minimum distance
  if (minIndex <= 320 && rotateDirSet == 0 )
    angularVel = 0.4;
  else if (rotateDirSet == 0)
    angularVel = -0.4;
  // set flag to ...