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; }
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()); args->add("-remoteRobotTcpPort"); // pass robot's TCP port to Aria args->add(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; } // Start dynamic_reconfigure server dynamic_reconfigure_server = new dynamic_reconfigure::Server<rosaria::RosAriaConfig>; robot->lock(); // Setup Parameter Minimums rosaria::RosAriaConfig dynConf_min; //arbitrary non-zero values so dynamic reconfigure isn't STUPID dynConf_min.trans_vel_max = 0.1; dynConf_min.rot_vel_max = 0.1; dynConf_min.trans_accel = 0.1; dynConf_min.trans_decel = 0.1; dynConf_min.rot_accel = 0.1; dynConf_min.rot_decel = 0.1; // 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); rosaria::RosAriaConfig dynConf_max; dynConf_max.trans_vel_max = robot->getAbsoluteMaxTransVel() / 1000.0; dynConf_max.rot_vel_max = robot->getAbsoluteMaxRotVel() *M_PI/180.0; dynConf_max.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000.0; dynConf_max.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000.0; dynConf_max.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180.0; dynConf_max.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180.0; // 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); dynConf_default.trans_vel_max = robot->getTransVelMax() / 1000.0; dynConf_default.rot_vel_max = robot->getRotVelMax() *M_PI/180.0; dynConf_default.trans_accel = robot->getTransAccel() / 1000.0; dynConf_default.trans_decel = robot->getTransDecel() / 1000.0; dynConf_default.rot_accel = robot->getRotAccel() * M_PI/180.0; dynConf_default.rot_decel = robot->getRotDecel() * M_PI/180.0; /* ROS_ERROR("ON ROBOT NOW\n\ Trans vel max: %f\n\ Rot vel max: %f\n\ \n\ trans accel: %f\n\ trans decel: %f\n\ rot accel: %f\n\ rot decel: %f", robot->getTransVelMax(), robot->getRotVelMax(), robot->getTransAccel(), robot->getTransDecel(), robot->getRotAccel(), robot->getRotDecel()); ROS_ERROR("IN DEFAULT CONFIG\n\ Trans vel max: %f\n\ Rot vel max: %f\n\ \n\ trans accel: %f\n\ trans decel: %f\n\ rot accel: %f\n\ rot decel: %f\n", dynConf_default.trans_vel_max, dynConf_default.rot_vel_max, dynConf_default.trans_accel, dynConf_default.trans_decel, dynConf_default.rot_accel, dynConf_default.rot_decel);*/ TicksMM = robot->getOrigRobotConfig()->getTicksMM(); DriftFactor = robot->getOrigRobotConfig()->getDriftFactor(); RevCount = robot->getOrigRobotConfig()->getRevCount(); dynConf_default.TicksMM = TicksMM; dynConf_default.DriftFactor = DriftFactor; dynConf_default.RevCount = RevCount; dynamic_reconfigure_server->setConfigDefault(dynConf_default); for(int i = 0; i < 16; i++) { sonar_tf_array[i].header.frame_id = frame_id_base_link; std::stringstream _frame_id; _frame_id << "sonar" << i; sonar_tf_array[i].child_frame_id = _frame_id.str(); ArSensorReading* _reading = NULL; _reading = robot->getSonarReading(i); sonar_tf_array[i].transform.translation.x = _reading->getSensorX() / 1000.0; sonar_tf_array[i].transform.translation.y = _reading->getSensorY() / 1000.0; sonar_tf_array[i].transform.translation.z = 0.19; sonar_tf_array[i].transform.rotation = tf::createQuaternionMsgFromYaw(_reading->getSensorTh() * M_PI / 180.0); } for (int i=0;i<16;i++) { sensor_msgs::Range r; ranges.data.push_back(r); } int i=0,j=0; if (sonars__crossed_the_streams) { i=8; j=8; } for(; i<16; i++) { //populate the RangeArray msg std::stringstream _frame_id; _frame_id << "sonar" << i; ranges.data[i].header.frame_id = _frame_id.str(); ranges.data[i].radiation_type = 0; ranges.data[i].field_of_view = 0.2618f; ranges.data[i].min_range = 0.03f; ranges.data[i].max_range = 5.0f; } // Enable the motors robot->enableMotors(); robot->disableSonar(); // Initialize bumpers with robot number of bumpers bumpers.front_bumpers.resize(robot->getNumFrontBumpers()); bumpers.rear_bumpers.resize(robot->getNumRearBumpers()); robot->unlock(); pose_pub = n.advertise<nav_msgs::Odometry>("pose",1000); bumpers_pub = n.advertise<rosaria::BumperState>("bumper_state",1000); voltage_pub = n.advertise<std_msgs::Float64>("battery_voltage", 1000); combined_range_pub = n.advertise<rosaria::RangeArray>("ranges", 1000, boost::bind(&RosAriaNode::sonarConnectCb,this), boost::bind(&RosAriaNode::sonarDisconnectCb, this)); for(int i =0; i < 16; i++) { std::stringstream topic_name; topic_name << "range" << i; range_pub[i] = n.advertise<sensor_msgs::Range>(topic_name.str().c_str(), 1000, boost::bind(&RosAriaNode::sonarConnectCb,this), boost::bind(&RosAriaNode::sonarDisconnectCb, this)); } recharge_state_pub = n.advertise<std_msgs::Int8>("battery_recharge_state", 5, true /*latch*/ ); recharge_state.data = -2; state_of_charge_pub = n.advertise<std_msgs::Float32>("battery_state_of_charge", 100); motors_state_pub = n.advertise<std_msgs::Bool>("motors_state", 5, true /*latch*/ ); motors_state.data = false; published_motors_state = false; // subscribe to services cmdvel_sub = n.subscribe( "cmd_vel", 1, (boost::function <void(const geometry_msgs::TwistConstPtr&)>) boost::bind(&RosAriaNode::cmdvel_cb, this, _1 )); // advertise enable/disable services enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this); disable_srv = n.advertiseService("disable_motors", &RosAriaNode::disable_motors_cb, this); veltime = ros::Time::now(); sonar_tf_timer = n.createTimer(ros::Duration(0.033), &RosAriaNode::sonarCallback, this); sonar_tf_timer.stop(); dynamic_reconfigure_server->setCallback(boost::bind(&RosAriaNode::dynamic_reconfigureCB, this, _1, _2)); // callback will be called by ArRobot background processing thread for every SIP data packet received from robot robot->addSensorInterpTask("ROSPublishingTask", 100, &myPublishCB); // Run ArRobot background processing thread robot->runAsync(true); return 0; }