int RosAriaNode::Setup()
{
// Note, various objects are allocated here which are never deleted (freed), since Setup() is only supposed to be
// called once per instance, and these objects need to persist until the process terminates.
robot = new ArRobot();
ArArgumentBuilder *args = new ArArgumentBuilder(); // never freed
ArArgumentParser *argparser = new ArArgumentParser(args); // Warning never freed
argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args. Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
// Now add any parameters given via ros params (see RosAriaNode constructor):
// if serial port parameter contains a ':' character, then interpret it as hostname:tcpport
// for wireless serial connection. Otherwise, interpret it as a serial port name.
size_t colon_pos = serial_port.find(":");
if (colon_pos != std::string::npos)
{
args->add("-remoteHost"); // pass robot's hostname/IP address to Aria
args->add(serial_port.substr(0, colon_pos).c_str());
//ROS_INFO( "RosAria: using IP: [%s]", serial_port.substr(0, colon_pos).c_str() );
args->add("-remoteRobotTcpPort"); // pass robot's TCP port to Aria
args->add(serial_port.substr(colon_pos+1).c_str());
//ROS_INFO( "RosAria: using port: [%s]", serial_port.substr(colon_pos+1).c_str() );
}
else
{
args->add("-robotPort"); // pass robot's serial port to Aria
args->add(serial_port.c_str());
}
// if a baud rate was specified in baud parameter
if(serial_baud != 0)
{
args->add("-robotBaud");
char tmp[100];
snprintf(tmp, 100, "%d", serial_baud);
args->add(tmp);
}
if( debug_aria )
{
// turn on all ARIA debugging
args->add("-robotLogPacketsReceived"); // log received packets
args->add("-robotLogPacketsSent"); // log sent packets
args->add("-robotLogVelocitiesReceived"); // log received velocities
args->add("-robotLogMovementSent");
args->add("-robotLogMovementReceived");
ArLog::init(ArLog::File, ArLog::Verbose, aria_log_filename.c_str(), true);
}
// Connect to the robot
conn = new ArRobotConnector(argparser, robot); // warning never freed
if (!conn->connectRobot()) {
ROS_ERROR("RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device.)");
return 1;
}
// causes ARIA to load various robot-specific hardware parameters from the robot parameter file in /usr/local/Aria/params
if(!Aria::parseArgs())
{
ROS_ERROR("RosAria: ARIA error parsing ARIA startup parameters!");
return 1;
}
readParameters();
// Start dynamic_reconfigure server
dynamic_reconfigure_server = new dynamic_reconfigure::Server<rosaria::RosAriaConfig>;
/*
* 横向速度和纵向速度单位为米
*/
/*
* 初始化参数的最小值 其中TickMM=10 DriftFactor=-200 Revount=-32760
*/
// Setup Parameter Minimums
rosaria::RosAriaConfig dynConf_min;
dynConf_min.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000;
dynConf_min.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000;
// TODO: Fix rqt dynamic_reconfigure gui to handle empty intervals
// Until then, set unit length interval.
dynConf_min.lat_accel = ((robot->getAbsoluteMaxLatAccel() > 0.0) ? robot->getAbsoluteMaxLatAccel() : 0.1) / 1000;
dynConf_min.lat_decel = ((robot->getAbsoluteMaxLatDecel() > 0.0) ? robot->getAbsoluteMaxLatDecel() : 0.1) / 1000;
dynConf_min.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180;
dynConf_min.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180;
// I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
dynConf_min.TicksMM = 10;
dynConf_min.DriftFactor = -200;
dynConf_min.RevCount = -32760;
dynamic_reconfigure_server->setConfigMin(dynConf_min);
// 初始化参数的最大值 其中TickMM=200 DriftFactor=200 Revount=32760
rosaria::RosAriaConfig dynConf_max;
dynConf_max.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000;
dynConf_max.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000;
// TODO: Fix rqt dynamic_reconfigure gui to handle empty intervals
// Until then, set unit length interval.
dynConf_max.lat_accel = ((robot->getAbsoluteMaxLatAccel() > 0.0) ? robot->getAbsoluteMaxLatAccel() : 0.1) / 1000;
dynConf_max.lat_decel = ((robot->getAbsoluteMaxLatDecel() > 0.0) ? robot->getAbsoluteMaxLatDecel() : 0.1) / 1000;
dynConf_max.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180;
dynConf_max.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180;
// I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
dynConf_max.TicksMM = 200;
dynConf_max.DriftFactor = 200;
dynConf_max.RevCount = 32760;
dynamic_reconfigure_server->setConfigMax(dynConf_max);
// 初始化参数的默认值
rosaria::RosAriaConfig dynConf_default;
dynConf_default.trans_accel = robot->getTransAccel() / 1000;
dynConf_default.trans_decel = robot->getTransDecel() / 1000;
dynConf_default.lat_accel = robot->getLatAccel() / 1000;
dynConf_default.lat_decel = robot->getLatDecel() / 1000;
dynConf_default.rot_accel = robot->getRotAccel() * M_PI/180;
dynConf_default.rot_decel = robot->getRotDecel() * M_PI/180;
dynConf_default.TicksMM = TicksMM;
dynConf_default.DriftFactor = DriftFactor;
dynConf_default.RevCount = RevCount;
dynamic_reconfigure_server->setConfigDefault(dynConf_max);
dynamic_reconfigure_server->setCallback(boost::bind(&RosAriaNode::dynamic_reconfigureCB, this, _1, _2));
// Enable the motors
robot->enableMotors();
// disable sonars on startup
robot->disableSonar();
// callback will be called by ArRobot background processing thread for every SIP data packet received from robot
robot->addSensorInterpTask("ROSPublishingTask", 100, &myPublishCB);
// Initialize bumpers with robot number of bumpers
bumpers.front_bumpers.resize(robot->getNumFrontBumpers());
bumpers.rear_bumpers.resize(robot->getNumRearBumpers());
// Run ArRobot background processing thread
robot->runAsync(true);
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();
}
