int main(int argc, char **argv)
{
// robot
ArRobot robot;
// the laser
ArSick sick;
// sonar, must be added to the robot
//ArSonarDevice sonar;
// the actions we'll use to wander
// recover from stalls
//ArActionStallRecover recover;
// react to bumpers
//ArActionBumpers bumpers;
// limiter for close obstacles
ArActionLimiterForwards limiter("speed limiter near", 1600, 0, 0, 1.3);
// limiter for far away obstacles
//ArActionLimiterForwards limiterFar("speed limiter near", 300, 1000, 450, 1.1);
//ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 600, 1.1);
// limiter for the table sensors
//ArActionLimiterTableSensor tableLimiter;
// actually move the robot
ArActionConstantVelocity constantVelocity("Constant Velocity", 1500);
// turn the orbot if its slowed down
ArActionTurn turn;
// mandatory init
Aria::init();
// Parse all our args
ArSimpleConnector connector(&argc, argv);
if (!connector.parseArgs() || argc > 1)
{
connector.logOptions();
exit(1);
}
// add the sonar to the robot
//robot.addRangeDevice(&sonar);
// add the laser to the robot
robot.addRangeDevice(&sick);
// try to connect, if we fail exit
if (!connector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
robot.comInt(ArCommands::SONAR, 0);
// turn on the motors, turn off amigobot sounds
//robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::SOUNDTOG, 0);
// add the actions
//robot.addAction(&recover, 100);
//robot.addAction(&bumpers, 75);
robot.addAction(&limiter, 49);
//robot.addAction(&limiter, 48);
//robot.addAction(&tableLimiter, 50);
robot.addAction(&turn, 30);
robot.addAction(&constantVelocity, 20);
robot.setStateReflectionRefreshTime(50);
limiter.activate();
turn.activate();
constantVelocity.activate();
robot.clearDirectMotion();
//robot.setStateReflectionRefreshTime(50);
robot.setRotVelMax(50);
robot.setTransAccel(1500);
robot.setTransDecel(100);
// start the robot running, true so that if we lose connection the run stops
robot.runAsync(true);
connector.setupLaser(&sick);
// now that we're connected to the robot, connect to the laser
sick.runAsync();
if (!sick.blockingConnect())
{
printf("Could not connect to SICK laser... exiting\n");
Aria::shutdown();
return 1;
}
sick.lockDevice();
sick.setMinRange(250);
sick.unlockDevice();
robot.lock();
ArGlobalFunctor1<ArRobot *> userTaskCB(&userTask, &robot);
robot.addUserTask("iotest", 100, &userTaskCB);
requestTime.setToNow();
robot.comInt(ArCommands::IOREQUEST, 1);
robot.comInt(ArCommands::ENABLE, 1);
robot.unlock();
robot.waitForRunExit();
// now exit
Aria::shutdown();
return 0;
}
/*
* dynamic reconfigure call back
*/
void RosAriaNode::dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level)
{
//
// Odometry Settings
//
robot->lock();
if(TicksMM != config.TicksMM and config.TicksMM > 0)
{
ROS_INFO("Setting TicksMM from Dynamic Reconfigure: %d -> %d ", TicksMM, config.TicksMM);
TicksMM = config.TicksMM;
robot->comInt(93, TicksMM);
}
if(DriftFactor != config.DriftFactor)
{
ROS_INFO("Setting DriftFactor from Dynamic Reconfigure: %d -> %d ", DriftFactor, config.DriftFactor);
DriftFactor = config.DriftFactor;
robot->comInt(89, DriftFactor);
}
if(RevCount != config.RevCount and config.RevCount > 0)
{
ROS_INFO("Setting RevCount from Dynamic Reconfigure: %d -> %d ", RevCount, config.RevCount);
RevCount = config.RevCount;
robot->comInt(88, RevCount);
}
//
// Acceleration Parameters
//
int value;
value = config.trans_accel * 1000;
if(value != robot->getTransAccel() and value > 0)
{
ROS_INFO("Setting TransAccel from Dynamic Reconfigure: %d", value);
robot->setTransAccel(value);
}
value = config.trans_decel * 1000;
if(value != robot->getTransDecel() and value > 0)
{
ROS_INFO("Setting TransDecel from Dynamic Reconfigure: %d", value);
robot->setTransDecel(value);
}
value = config.lat_accel * 1000;
if(value != robot->getLatAccel() and value > 0)
{
ROS_INFO("Setting LatAccel from Dynamic Reconfigure: %d", value);
if (robot->getAbsoluteMaxLatAccel() > 0 )
robot->setLatAccel(value);
}
value = config.lat_decel * 1000;
if(value != robot->getLatDecel() and value > 0)
{
ROS_INFO("Setting LatDecel from Dynamic Reconfigure: %d", value);
if (robot->getAbsoluteMaxLatDecel() > 0 )
robot->setLatDecel(value);
}
value = config.rot_accel * 180/M_PI;
if(value != robot->getRotAccel() and value > 0)
{
ROS_INFO("Setting RotAccel from Dynamic Reconfigure: %d", value);
robot->setRotAccel(value);
}
value = config.rot_decel * 180/M_PI;
if(value != robot->getRotDecel() and value > 0)
{
ROS_INFO("Setting RotDecel from Dynamic Reconfigure: %d", value);
robot->setRotDecel(value);
}
robot->unlock();
}
void RosAriaNode::dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level)
{
//ROS_INFO("Dynamic reconfigure callback fired!");
//
// Odometry Settings
//
robot->lock();
if(TicksMM != config.TicksMM and config.TicksMM > 0)
{
ROS_INFO("Setting TicksMM from Dynamic Reconfigure: %d -> %d ", TicksMM, config.TicksMM);
TicksMM = config.TicksMM;
robot->comInt(93, TicksMM);
}
if(DriftFactor != config.DriftFactor)
{
ROS_INFO("Setting DriftFactor from Dynamic Reconfigure: %d -> %d ", DriftFactor, config.DriftFactor);
DriftFactor = config.DriftFactor;
robot->comInt(89, DriftFactor);
}
if(RevCount != config.RevCount and config.RevCount > 0)
{
ROS_INFO("Setting RevCount from Dynamic Reconfigure: %d -> %d ", RevCount, config.RevCount);
RevCount = config.RevCount;
robot->comInt(88, RevCount);
}
////
//// WHEN ROSPARAM INITIALIZES WITH THE DEFAULT VALUES, IT FILLS IN ONE NON-ZERO VALUE AT A TIME
//// CONSEQUENTLY, THE SAME _CORRECT_ VALUE IS SET TO MAKE SURE THAT parameter_updates, AND ROSPARAM
//// ALL REFLECT THE PROGRAMATICALLY SET DEFAULT VALUES AFTER INITIALIZATION COMPLETES
////
//
// Acceleration Parameters
//
double value;
value = config.trans_accel * 1000.0;
if(value != robot->getTransAccel() and value > 0)
{
ROS_INFO("Setting TransAccel from Dynamic Reconfigure: %f m/s^2", config.trans_accel);
robot->setTransAccel(value);
}
else if (value == 0)
setDynParam("trans_accel", dynConf_default.trans_accel);
value = config.trans_decel * 1000.0;
if(value != robot->getTransDecel() and value > 0)
{
ROS_INFO("Setting TransDecel from Dynamic Reconfigure: %f m/s^2", config.trans_decel);
robot->setTransDecel(value);
}
else if (value == 0)
setDynParam("trans_decel", dynConf_default.trans_decel);
value = config.rot_accel * 180.0/M_PI;
if(value != robot->getRotAccel() and value > 0)
{
ROS_INFO("Setting RotAccel from Dynamic Reconfigure: %f radians/s^2", config.rot_accel);
robot->setRotAccel(value);
}
else if (value == 0)
setDynParam("rot_accel", dynConf_default.rot_accel);
value = config.rot_decel * 180.0/M_PI;
if(value != robot->getRotDecel() and value > 0)
{
ROS_INFO("Setting RotDecel from Dynamic Reconfigure: %f radians/s^2", config.rot_decel);
robot->setRotDecel(value);
}
else if (value == 0)
setDynParam("rot_decel", dynConf_default.rot_decel);
value = config.rot_vel_max * 180.0/M_PI;
if(value != robot->getRotVelMax() and value > 0)
{
ROS_INFO("Setting RotVelMax from Dynamic Reconfigure: %f radians/s", config.rot_vel_max);
robot->setRotVelMax(value);
}
else if (value == 0)
setDynParam("rot_vel_max", dynConf_default.rot_vel_max);
value=config.trans_vel_max * 1000.0;
if (value != robot->getTransVelMax() and value > 0)
{
ROS_INFO("Setting TransVelMax from Dynamic Reconfigure: %f m/s", config.trans_vel_max);
robot->setTransVelMax(value);
}
else if (value == 0)
setDynParam("trans_vel_max", dynConf_default.trans_vel_max);
robot->unlock();
}
