vel=0 when ROS is disconnected
@@ -86,7 +86,7 @@ void loop()
if ((t-tTime[0]) >= (1000 / CONTROL_MOTOR_SPEED_FREQUENCY))
{
- updateGoalVelocity();
+ updateGoalVelocity(nh.connected());
motor_driver.controlMotor(WHEEL_RADIUS, WHEEL_SEPARATION, goal_velocity);
tTime[0] = t;
}
@@ -931,8 +931,12 @@ void initJointStates(void)
/*******************************************************************************
* Update Goal Velocity
*******************************************************************************/
-void updateGoalVelocity(void)
+void updateGoalVelocity(bool isConnected)
{
+ if(!isConnected){
+ goal_velocity_from_cmd[LINEAR] = 0.0;
+ goal_velocity_from_cmd[ANGULAR] = 0.0;
+ }
goal_velocity[LINEAR] = goal_velocity_from_button[LINEAR] + goal_velocity_from_cmd[LINEAR] + goal_velocity_from_rc100[LINEAR];
goal_velocity[ANGULAR] = goal_velocity_from_button[ANGULAR] + goal_velocity_from_cmd[ANGULAR] + goal_velocity_from_rc100[ANGULAR];
}
