I have a robot that uses an RPLIDAR for scanning and an arduino that controls BLDC motor via pwm. I have been able to create a map using GMAPPING, publish odom messages and subscribe my arduino to cmd_vel topic. My arduino calculates the pwm according to the velocity and rotates the wheel. What I am still unable to do is that my robots location doesnot change when I move it around or give it a 2d nav goal. The wheels just keep rotating and robot doesnot stop at the goal.

I URGENTLY NEED HELP I have no idea, what am I doing wrong?

Also, I launched by noxbringup file and view tf on rviz and the gave i a velocity via TELEOPKEYBOARD. I saw on rviz that my robot doesnot move straight even when i donot turn it.

these are my launch and yaml files and arduino code P.S. my code is a version of NOX ROBOT available on ROS website http://wiki.ros.org/Robots/Nox nox_navigation.launch:

<!-- ************** Navigation *************** -->

    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />

    <!-- param name="controller_frequency" value="5.0" />  -->
    <param name="controller_patience" value="15.0" />

        <param name="clearing_rotation_allowed" value="false" /> <!-- Nox is able to rotate in place -->
    <!-- <param name= "allow_none" value="True" /> -->
</node>

nox_bringup.launch:

************** Sensors ***************

commoncostmapparams.yaml:

footprint: [ [-0.27,-0.19], [0.13,-0.19], [0.13,0.19], [-0.27,0.19] ]

transformtolerance: 0.4 maptype: costmap

obstaclelayer: enabled: true obstaclerange: 5.0 raytracerange: 5.5 inflationradius: 0.1 trackunknownspace: false combination_method: 1

observationsources: laserscansensor laserscansensor: {datatype: LaserScan, topic: scan, marking: true, clearing: true}

inflationlayer: enabled: true costscalingfactor: 10.0
inflationradius: 0.1

staticlayer: enabled: true maptopic: "map"

globalcostmapparams.yaml:

globalcostmap: globalframe: map robotbaseframe: baselink updatefrequency: 1.0 publishfrequency: 0.5 staticmap: true

transformtolerance: 0.5 plugins: - {name: staticlayer, type: "costmap2d::StaticLayer"} - {name: obstaclelayer, type: "costmap2d::VoxelLayer"} - {name: inflationlayer, type: "costmap_2d::InflationLayer"}

localcostmapparams: localcostmap: globalframe: map robotbaseframe: baselink updatefrequency: 2.0 publishfrequency: 1.0 staticmap: false rollingwindow: true width: 10 height: 10 resolution: 0.1 transformtolerance: 0.3

plugins: - {name: staticlayer, type: "costmap2d::StaticLayer"} - {name: obstaclelayer, type: "costmap2d::ObstacleLayer"}

baselocalplanner_params.yaml:

TrajectoryPlannerROS: # for details see: http://www.ros.org/wiki/base_local_planner odom_topic: odom

maxvelx: 0.2229

minvelx: 0.2026

maxrotationalvel: 0.12 # 0.1 rad/sec = 5.7 degree/sec

mininplacerotationalvel: 0.04

acclimth: 0.12

acclimx: 0.05

acclimy: 0

holonomic_robot: false

# goal tolerance parameters

yawgoaltolerance: 0.4 # 0.1 means 5.7 degrees

xygoaltolerance: 0.1

latchxygoal_tolerance: false

# Forward Simulation Parameters

sim_time: 1

sim_granularity: 0.025

angularsimgranularity: 0.01

vx_samples: 10

vtheta_samples: 20

controller_frequency: 10

# Trajectory Scoring Parameters meter_scoring: true

pathdistancebias: 1.0

goaldistancebias: 0.8

occdist_scale: 0.01

heading_lookahead: 0.325

oscillationresetdist: 0.05

publishcostgrid: true

dwa: true

dwalocalplanner_params.yaml:

DWAPlannerROS:

odom_topic: odom

Robot Configuration Parameters

maxvelx: 0.2229

minvelx: 0.2026

maxvely: 0.0 # 0 for non-holonomic robot

minvely: 0.0 # 0 for non-holonomic robot

The velocity when robot is moving in a straight line

maxtransvel: 0.2229

mintransvel: 0.2026

maxrotvel: 0.03

minrotvel: 0.02

acclimx: 0.09

acclimy: 0.0

acclimtheta: 3.2

Goal Tolerance Parametes

xygoaltolerance: 0.1

yawgoaltolerance: 0.07

latchxygoal_tolerance: false

Forward Simulation Parameters

sim_time: 2.0

sim_granularity: 0.4

vx_samples: 20

vy_samples: 0

vtheta_samples: 40

controller_frequency: 10.0

Trajectory Scoring Parameters

pathdistancebias: 34.0

goaldistancebias: 20.0

occdist_scale: 0.02

forwardpointdistance: 0.325

stoptimebuffer: 0.2

scaling_speed: 0.25

maxscalingfactor: 0.2

Oscillation Prevention Parameters

oscillationresetdist: 0.05

Debugging

publishtrajpc : true

publishcostgrid_pc: true

teb local planner parameters:

TebLocalPlannerROS:

odomtopic: odom publishfeedback: True

# Trajectory

tebautosize: True dtref: 0.4

dt_hysteresis: 0.04

globalplanoverwrite_orientation: True

allowinitwithbackwardsmotion: True

maxglobalplanlookaheaddist: 6.0

feasibilitycheckno_poses: 2

# Robot

maxvelx: 0.04

maxvelx_backwards: 0.0

maxvely: 0.0

maxveltheta: 0.1

acclimx: 0.015

acclimtheta: 0.12

# ********************** Carlike robot parameters ******************** minturningradius: 0.734

wheelbase: 0.32

cmdangleinstead_rotvel: False

# ********************************************************************

footprintmodel: # types: "point", "circular", "twocircles", "line", "polygon"

type: "circular"

radius: 0.17 # for type "circular"

line_start: [-0.19, 0.0] # for type "line"

line_end: [0.14, 0.0] # for type "line"

frontoffset: 0.2 # for type "twocircles"

frontradius: 0.2 # for type "twocircles"

rearoffset: 0.2 # for type "twocircles"

rearradius: 0.2 # for type "twocircles"

vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ]

# GoalTolerance

xygoaltolerance: 0.05

yawgoaltolerance: 0.05

freegoalvel: False

# Obstacles

minobstacledist: 0.1

includecostmapobstacles: True

costmapobstaclesbehindrobotdist: 1.0

obstacleposesaffected: 30

costmapconverterplugin: ""

costmapconverterspin_thread: True

costmapconverterrate: 5

# Optimization

noinneriterations: 5

noouteriterations: 4

optimization_activate: True

optimization_verbose: False

penalty_epsilon: 0.1

weightmaxvel_x: 2

weightmaxvel_theta: 2

weightacclim_x: 1

weightacclim_theta: 1

weightkinematicsnh: 1000

weightkinematicsforward_drive: 1

weightkinematicsturning_radius: 1

weight_optimaltime: 1

weight_obstacle: 50

weightdynamicobstacle: 10 # not in use yet

# Homotopy Class Planner

enablehomotopyclass_planning: True

enable_multithreading: True

simple_exploration: False

maxnumberclasses: 4

selectioncosthysteresis: 1.0

selectionobstcost_scale: 1.0

selectionalternativetime_cost: False

roadmapgraphno_samples: 15

roadmapgrapharea_width: 5

hsignatureprescaler: 0.5

hsignaturethreshold: 0.1

obstaclekeypointoffset: 0.1

obstacleheadingthreshold: 0.45

visualizehcgraph: False

ARDUINO CODE:

include include include include include include include

//initializing all the variables define LOOPTIME 100 Looptime in millisecond const byte noCommLoopMax = 10;
unsigned int noCommLoops = 0; main loop without communication counter

double speedcmdleft2 = 0;

const int PINENCODAMOTORLEFT = 20; //A channel for encoder of left motor const int PINENCODBMOTORLEFT = 19; //B channel for encoder of left motor

const int PINENCODAMOTORRIGHT = 2; //A channel for encoder of right motor const int PINENCODBMOTORRIGHT = 3; //B channel for encoder of right motor

const int PINSIDELIGHT_LED = 46; //Side light blinking led pin

unsigned long lastMilli = 0;

//--- Robot-specific constants --- const double radius = 0.085; //Wheel radius, in m const double wheelbase = 0.35; //Wheelbase, in m const double encodercpr = 91; //Encoder ticks or counts per rotation const double speedtopwmratio = 0.00235;

const double minspeedcmd = 0.0882; //(minspeedcmd/speedtopwm_ratio)

double speedreq = 0; //Desired linear speed for the robot, in m/s double angularspeed_req = 0; //Desired angular speed for the robot, in rad/s

double speedreqleft = 0; //Desired speed for left wheel in m/s double speedactleft = 0; //Actual speed for left wheel in m/s double speedcmdleft = 0; //Command speed for left wheel in m/s

double speedreqright = 0; //Desired speed for right wheel in m/s double speedactright = 0; //Actual speed for right wheel in m/s double speedcmdright = 0; //Command speed for right wheel in m/s

const double max_speed = 0.1; //Max speed in m/s

int PWMleftMotor = 0; //PWM command for left motor int PWMrightMotor = 0; //PWM command for right motor

// PID Parameters const double PIDleftparam[] = { 0.011, 0, 0 }; //Respectively Kp, Ki and Kd for left motor PID const double PIDrightparam[] = { 0.03, 0, 0 }; //Respectively Kp, Ki and Kd for right motor PID

volatile float posleft = 0; //Left motor encoder position volatile float posright = 0; //Right motor encoder position

PID PIDleftMotor(&speedactleft, &speedcmdleft, &speedreqleft, PIDleftparam[0], PIDleft_param[1],

PIDleftparam[2], DIRECT); //Setting up the PID for left motor

PID PIDrightMotor(&speedactright, &speedcmdright, &speedreqright, PIDrightparam[0], PIDrightparam[1], PIDright_param[2], DIRECT); //Setting up the PID for right motor

ros::NodeHandle nh;

//function that will be called when receiving command from host

void handlecmd (const geometrymsgs::Twist& cmd_vel) { noCommLoops = 0; //Reset the counter for number of main loops without communication

speedreq = cmdvel.linear.x; //Extract the commanded linear speed from the message

angularspeedreq = cmd_vel.angular.z; //Extract the commanded angular speed from the message

speedreqleft = speedreq - angularspeedreq * (wheelbase / 2); //Calculate the required speed for the left motor to comply with commanded linear and angular speeds speedreqright = speedreq + angularspeedreq * (wheelbase / 2); //Calculate the required speed for the right motor to comply with commanded linear and angular speeds }

ros::Subscriber cmdvel("cmdvel", handle_cmd); //create a subscriber to ROS topic for velocity commands

geometrymsgs::Vector3Stamped speedmsg; //create a "speed_msg" ROS message

ros::Publisher speedpub("speed", &speedmsg); //create a publisher to ROS topic "speed" using the "speed_msg" type

void setup() {

nh.initNode();

nh.subscribe(cmd_vel);

nh.advertise(speed_pub);

//setting motor speeds to zero analogWrite(9, 0);

analogWrite(11, 0);

//setting PID parameters PID_leftMotor.SetSampleTime(95);

PID_rightMotor.SetSampleTime(95);

PIDleftMotor.SetOutputLimits(0.2, maxspeed);

PIDrightMotor.SetOutputLimits(0.2, maxspeed);

PID_leftMotor.SetMode(AUTOMATIC);

PID_rightMotor.SetMode(AUTOMATIC);

// Define the rotary encoder for left motor pinMode(PINENCODAMOTORLEFT, INPUT);

pinMode(PINENCODBMOTORLEFT, INPUT);

digitalWrite(PINENCODAMOTORLEFT, HIGH); // turn on pullup resistor

digitalWrite(PINENCODBMOTORLEFT, HIGH);

attachInterrupt(digitalPinToInterrupt(21), encoderLeftMotor, CHANGE);

// Define the rotary encoder for right motor pinMode(PINENCODAMOTORRIGHT, INPUT);

pinMode(PINENCODBMOTORRIGHT, INPUT);

digitalWrite(PINENCODAMOTORRIGHT, HIGH); // turn on pullup resistor

digitalWrite(PINENCODBMOTORRIGHT, HIGH);

attachInterrupt(digitalPinToInterrupt(18), encoderRightMotor, CHANGE);

}

//_________________________________________________________________________

void loop() { nh.spinOnce(); if ((millis() - lastMilli) >= LOOPTIME) { // enter timed loop lastMilli = millis();

if (abs(pos_left) < 5) {                                                  //Avoid taking in account small disturbances
  speed_act_left = 0;
}
else {
  speed_act_left = ((pos_left / encoder_cpr) * 2 * PI) * (1000 / LOOPTIME) * radius; // calculate speed of left wheel
}

if (abs(pos_right) < 5) {                                                //Avoid taking in account small disturbances

  speed_act_right = 0;

}

else {

  speed_act_right = ((pos_right / encoder_cpr) * 2 * PI) * (1000 / LOOPTIME) * radius; // calculate speed of right wheel

}

pos_left = 0;

pos_right = 0;

speed_cmd_left = constrain(speed_cmd_left, 0, max_speed);

PID_leftMotor.Compute();   // compute PWM value for left motor

PWM_leftMotor = constrain(((speed_req_left + sgn(speed_req_left) * min_speed_cmd) / speed_to_pwm_ratio) + (speed_cmd_left / speed_to_pwm_ratio), -255, 255); //

if (noCommLoops >= noCommLoopMax) {                   //Stopping if too much time without command

  analogWrite(9, 0);

}
else if (speed_req_left == 0) {                       //Stopping

  analogWrite(9, 0);

}
else if (PWM_leftMotor > 0) {                         //Going forward

  analogWrite(9, abs(PWM_leftMotor));

}

speed_cmd_right = constrain(speed_cmd_right, 0, max_speed);

PID_rightMotor.Compute();   // compute PWM value for right motor.

PWM_rightMotor = constrain(((speed_req_right + sgn(speed_req_right) * min_speed_cmd) / speed_to_pwm_ratio) + (speed_cmd_right / speed_to_pwm_ratio), 110, 255); 


if (noCommLoops >= noCommLoopMax) {                   //Stopping if too much time without command

analogWrite(11, 0);

}
else if (speed_req_right == 0) {                       //Stopping

  analogWrite(11, 0);

}
else if (PWM_rightMotor > 0) {                        //Going forward

  analogWrite(11, abs(PWM_rightMotor));

}

if ((millis() - lastMilli) >= LOOPTIME) {     //write an error if execution time of the loop in longer than the specified looptime

 // Serial.println(" TOO LONG ");

}

noCommLoops++;

if (noCommLoops == 65535) {

  noCommLoops = noCommLoopMax;

}

publishSpeed(LOOPTIME);   //Publish odometry on ROS topic

} }

void publishSpeed(double time) {

speed_msg.header.stamp = nh.now(); //timestamp for odometry data

speedmsg.vector.x = speedact_left; //left wheel speed (in m/s)

speedmsg.vector.y = speedact_right; //right wheel speed (in m/s)

speed_msg.vector.z = time / 100; //looptime, should be the same as specified in LOOPTIME (in s)

speedpub.publish(&speedmsg);

nh.spinOnce();

nh.loginfo("Publishing odometry");

}

//Left motor encoder counter void encoderLeftMotor() {

if (digitalRead(PINENCODAMOTORLEFT) == digitalRead(PINENCODBMOTORLEFT)) pos_left++;

else pos_left--;

}

//Right motor encoder counter void encoderRightMotor() {

if (digitalRead(PINENCODAMOTORRIGHT) == digitalRead(PINENCODBMOTORRIGHT)) pos_right--;

else pos_right++;

}

}