void addGoalFinished(ArPose pose) { ArLog::log(ArLog::Normal, "Finished at x = %f, y = %f", pose.getX(), pose.getY()); }
void RosAriaNode::publish() { // Note, this is called via SensorInterpTask callback (myPublishCB, named "ROSPublishingTask"). ArRobot object 'robot' sholud not be locked or unlocked. pos = robot->getPose(); tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(pos.getTh()*M_PI/180), tf::Vector3(pos.getX()/1000, pos.getY()/1000, 0)), position.pose.pose); //Aria returns pose in mm. position.twist.twist.linear.x = robot->getVel()/1000; //Aria returns velocity in mm/s. position.twist.twist.linear.y = robot->getLatVel()/1000.0; position.twist.twist.angular.z = robot->getRotVel()*M_PI/180; position.header.frame_id = frame_id_odom; position.child_frame_id = frame_id_base_link; position.header.stamp = ros::Time::now(); pose_pub.publish(position); ROS_DEBUG("RosAria: publish: (time %f) pose x: %f, y: %f, angle: %f; linear vel x: %f, y: %f; angular vel z: %f", position.header.stamp.toSec(), (double)position.pose.pose.position.x, (double)position.pose.pose.position.y, (double)position.pose.pose.orientation.w, (double) position.twist.twist.linear.x, (double) position.twist.twist.linear.y, (double) position.twist.twist.angular.z ); // publishing transform odom->base_link odom_trans.header.stamp = ros::Time::now(); odom_trans.header.frame_id = frame_id_odom; odom_trans.child_frame_id = frame_id_base_link; odom_trans.transform.translation.x = pos.getX()/1000; odom_trans.transform.translation.y = pos.getY()/1000; odom_trans.transform.translation.z = 0.0; odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pos.getTh()*M_PI/180); odom_broadcaster.sendTransform(odom_trans); // getStallValue returns 2 bytes with stall bit and bumper bits, packed as (00 00 FrontBumpers RearBumpers) int stall = robot->getStallValue(); unsigned char front_bumpers = (unsigned char)(stall >> 8); unsigned char rear_bumpers = (unsigned char)(stall); bumpers.header.frame_id = frame_id_bumper; bumpers.header.stamp = ros::Time::now(); std::stringstream bumper_info(std::stringstream::out); // Bit 0 is for stall, next bits are for bumpers (leftmost is LSB) for (unsigned int i=0; i<robot->getNumFrontBumpers(); i++) { bumpers.front_bumpers[i] = (front_bumpers & (1 << (i+1))) == 0 ? 0 : 1; bumper_info << " " << (front_bumpers & (1 << (i+1))); } ROS_DEBUG("RosAria: Front bumpers:%s", bumper_info.str().c_str()); bumper_info.str(""); // Rear bumpers have reverse order (rightmost is LSB) unsigned int numRearBumpers = robot->getNumRearBumpers(); for (unsigned int i=0; i<numRearBumpers; i++) { bumpers.rear_bumpers[i] = (rear_bumpers & (1 << (numRearBumpers-i))) == 0 ? 0 : 1; bumper_info << " " << (rear_bumpers & (1 << (numRearBumpers-i))); } ROS_DEBUG("RosAria: Rear bumpers:%s", bumper_info.str().c_str()); bumpers_pub.publish(bumpers); //Publish battery information // TODO: Decide if BatteryVoltageNow (normalized to (0,12)V) is a better option std_msgs::Float64 batteryVoltage; batteryVoltage.data = robot->getRealBatteryVoltageNow(); voltage_pub.publish(batteryVoltage); if(robot->haveStateOfCharge()) { std_msgs::Float32 soc; soc.data = robot->getStateOfCharge()/100.0; state_of_charge_pub.publish(soc); } // publish recharge state if changed char s = robot->getChargeState(); if(s != recharge_state.data) { ROS_INFO("RosAria: publishing new recharge state %d.", s); recharge_state.data = s; recharge_state_pub.publish(recharge_state); } // publish motors state if changed bool e = robot->areMotorsEnabled(); if(e != motors_state.data || !published_motors_state) { ROS_INFO("RosAria: publishing new motors state %d.", e); motors_state.data = e; motors_state_pub.publish(motors_state); published_motors_state = true; } // Publish sonar information, if enabled. if (use_sonar) { sensor_msgs::PointCloud cloud; //sonar readings. cloud.header.stamp = position.header.stamp; //copy time. // sonar sensors relative to base_link cloud.header.frame_id = frame_id_sonar; // Log debugging info std::stringstream sonar_debug_info; sonar_debug_info << "Sonar readings: "; for (int i = 0; i < robot->getNumSonar(); i++) { ArSensorReading* reading = NULL; reading = robot->getSonarReading(i); if(!reading) { ROS_WARN("RosAria: Did not receive a sonar reading."); continue; } // getRange() will return an integer between 0 and 5000 (5m) sonar_debug_info << reading->getRange() << " "; // local (x,y). Appears to be from the centre of the robot, since values may // exceed 5000. This is good, since it means we only need 1 transform. // x & y seem to be swapped though, i.e. if the robot is driving north // x is north/south and y is east/west. // //ArPose sensor = reading->getSensorPosition(); //position of sensor. // sonar_debug_info << "(" << reading->getLocalX() // << ", " << reading->getLocalY() // << ") from (" << sensor.getX() << ", " // << sensor.getY() << ") ;; " ; //add sonar readings (robot-local coordinate frame) to cloud geometry_msgs::Point32 p; p.x = reading->getLocalX() / 1000.0; p.y = reading->getLocalY() / 1000.0; p.z = 0.0; cloud.points.push_back(p); } ROS_DEBUG_STREAM(sonar_debug_info.str()); sonar_pub.publish(cloud); } }
int main(int argc, char **argv) { Aria::init(); ArLog::init(ArLog::StdOut, ArLog::Verbose); ArMap testMap; ArTime timer; ArGlobalFunctor mapChangedCB(&mapChanged); testMap.addMapChangedCB(&mapChangedCB); if (argc <= 1) { printf("mapTest: Usage %s <map> <map2:optional>\n", argv[0]); Aria::exit(1); } timer.setToNow(); if (!testMap.readFile(argv[1])) { printf("mapTest: Could not read map '%s'\n", argv[1]); Aria::exit(2); } printf("mapTest: Took %ld ms to read file\n", timer.mSecSince()); /* printf("mapTest: ChangeTimes (in ms): mapObjects %ld points %ld mapInfo %ld\n", testMap.getMapObjectsChanged().mSecSince(), testMap.getPointsChanged().mSecSince(), testMap.getMapInfoChanged().mSecSince()); */ timer.setToNow(); if(!testMap.writeFile("mapTest.map")) { printf("mapTest: Error could not write new map to mapTest.map"); Aria::exit(3); } printf("mapTest: Took %ld ms to write file mapTest.map\n", timer.mSecSince()); std::list<ArMapObject *>::iterator objIt; ArMapObject *obj; for (objIt = testMap.getMapObjects()->begin(); objIt != testMap.getMapObjects()->end(); objIt++) { obj = (*objIt); printf("mapTest: Map object: %s named \"%s\". Pose: %0.2f,%0.2f,%0.2f. ", obj->getType(), obj->getName(), obj->getPose().getX(), obj->getPose().getY(), obj->getPose().getTh()); if(obj->hasFromTo()) printf("mapTest: Extents: From %0.2f,%0.2f to %0.2f,%0.2f.", obj->getFromPose().getX(), obj->getFromPose().getY(), obj->getToPose().getX(), obj->getToPose().getY()); printf("mapTest: \n"); /* if (strcasecmp(obj->getType(), "Goal") == 0 || strcasecmp(obj->getType(), "GoalWithHeading") == 0) { printf("mapTest: Map object: Goal %s\n", obj->getName()); } else if (strcasecmp(obj->getType(), "ForbiddenLine") == 0 && obj->hasFromTo()) { printf("mapTest: Map object: Forbidden line from %.0f %.0f to %.0f %.0f\n", obj->getFromPose().getX(), obj->getFromPose().getY(), obj->getToPose().getX(), obj->getToPose().getY()); } */ } std::list<ArArgumentBuilder*>* objInfo = testMap.getMapInfo(); for(std::list<ArArgumentBuilder*>::const_iterator i = objInfo->begin(); i != objInfo->end(); i++) { printf("mapTest: MapInfo object definition:\n----\n"); (*i)->log(); printf("mapTest: ----\n"); } printf("mapTest: First 5 data points:\n"); std::vector<ArPose>::iterator pIt; ArPose pose; int n = 0; for (pIt = testMap.getPoints()->begin(); pIt != testMap.getPoints()->end(); pIt++) { pose = (*pIt); if (n > 5) exit(0); printf("mapTest: \t%.0f %.0f\n", pose.getX(), pose.getY()); n++; // the points are gone through } if (argc >= 3) { timer.setToNow(); if (!testMap.readFile(argv[2])) { printf("mapTest: Could not read map '%s'\n", argv[2]); } printf("mapTest: Took %ld ms to read file2\n", timer.mSecSince()); /* ArUtil::sleep(30); printf("mapTest: ChangeTimes (in ms): mapObjects %ld points %ld mapInfo %ld\n", testMap.getMapObjectsChanged().mSecSince(), testMap.getPointsChanged().mSecSince(), testMap.getMapInfoChanged().mSecSince()); */ timer.setToNow(); testMap.writeFile("mapTest2.map"); printf("mapTest: Took %ld ms to write file2\n", timer.mSecSince()); } // now test it with the config stuff ArArgumentBuilder builder; builder.setExtraString("Map"); builder.add(argv[1]); printf("mapTest: Trying config with map (%s)\n", argv[1]); Aria::getConfig()->parseArgument(&builder); Aria::getConfig()->callProcessFileCallBacks(true, NULL, 0); printf("mapTest: Trying config again with same map (%s)\n", argv[1]); Aria::getConfig()->parseArgument(&builder); Aria::getConfig()->callProcessFileCallBacks(true, NULL, 0); if (argc >= 3) { ArArgumentBuilder builder2; builder2.setExtraString("Map"); builder2.add(argv[2]); printf("mapTest: Trying config with map2 (%s)\n", argv[2]); Aria::getConfig()->parseArgument(&builder2); Aria::getConfig()->callProcessFileCallBacks(true, NULL, 0); } }
/*------------------------------------------------------------- getOdometryIncrement -------------------------------------------------------------*/ void CActivMediaRobotBase::getOdometryIncrement( poses::CPose2D &out_incr_odom, double &out_lin_vel, double &out_ang_vel, int64_t &out_incr_left_encoder_ticks, int64_t &out_incr_right_encoder_ticks ) { #if MRPT_HAS_ARIA ASSERTMSG_(THE_ROBOT!=NULL, "Robot is not connected") THE_ROBOT->lock(); static CPose2D last_pose; static int64_t last_left_ticks=0, last_right_ticks=0; CPose2D cur_pose; int64_t cur_left_ticks, cur_right_ticks; // Velocities: out_lin_vel = THE_ROBOT->getVel() * 0.001; out_ang_vel = DEG2RAD( THE_ROBOT->getRotVel() ); // Current odometry: ArPose pose = THE_ROBOT->getEncoderPose(); cur_pose.x( pose.getX() * 0.001 ); cur_pose.y( pose.getY() * 0.001 ); cur_pose.phi( DEG2RAD( pose.getTh() ) ); // Current encoders: cur_left_ticks = THE_ROBOT->getLeftEncoder(); cur_right_ticks = THE_ROBOT->getRightEncoder(); // Compute increment: if (m_firstIncreOdometry) { // First time: m_firstIncreOdometry = false; out_incr_odom = CPose2D(0,0,0); out_incr_left_encoder_ticks = 0; out_incr_right_encoder_ticks = 0; } else { // Normal case: out_incr_odom = cur_pose - last_pose; out_incr_left_encoder_ticks = cur_left_ticks - last_left_ticks; out_incr_right_encoder_ticks = cur_right_ticks - last_right_ticks; } // save for the next time: last_pose = cur_pose; last_left_ticks = cur_left_ticks; last_right_ticks = cur_right_ticks; THE_ROBOT->unlock(); #else MRPT_UNUSED_PARAM(out_incr_odom); MRPT_UNUSED_PARAM(out_lin_vel); MRPT_UNUSED_PARAM(out_ang_vel); MRPT_UNUSED_PARAM(out_incr_left_encoder_ticks); MRPT_UNUSED_PARAM(out_incr_right_encoder_ticks); THROW_EXCEPTION("MRPT has been compiled with 'MRPT_BUILD_ARIA'=OFF, so this class cannot be used."); #endif }
AREXPORT void ArForbiddenRangeDevice::processReadings(void) { ArPose intersection; std::list<ArLineSegment *>::iterator it; lockDevice(); myDataMutex.lock(); myCurrentBuffer.beginRedoBuffer(); if (!myIsEnabled) { myCurrentBuffer.endRedoBuffer(); myDataMutex.unlock(); unlockDevice(); return; } ArLineSegment *segment; ArPose start; double startX; double startY; ArPose end; double angle; double length; double gone; double sin; double cos; double atX; double atY; double robotX = myRobot->getX(); double robotY = myRobot->getY(); double max = (double) myMaxRange; double maxSquared = (double) myMaxRange * (double) myMaxRange; ArTime startingTime; //startingTime.setToNow(); // now see if the end points of the segments are too close to us for (it = mySegments.begin(); it != mySegments.end(); it++) { segment = (*it); // if either end point or some perpindicular point is close to us // add the line's data if (ArMath::squaredDistanceBetween( segment->getX1(), segment->getY1(), myRobot->getX(), myRobot->getY()) < maxSquared || ArMath::squaredDistanceBetween( segment->getX2(), segment->getY2(), myRobot->getX(), myRobot->getY()) < maxSquared || segment->getPerpDist(myRobot->getPose()) < max) { start.setPose(segment->getX1(), segment->getY1()); end.setPose(segment->getX2(), segment->getY2()); angle = start.findAngleTo(end); cos = ArMath::cos(angle); sin = ArMath::sin(angle); startX = start.getX(); startY = start.getY(); length = start.findDistanceTo(end); // first put in the start point if we should if (ArMath::squaredDistanceBetween( startX, startY, robotX, robotY) < maxSquared) myCurrentBuffer.redoReading(start.getX(), start.getY()); // now walk the length of the line and see if we should put the points in for (gone = 0; gone < length; gone += myDistanceIncrement) { atX = startX + gone * cos; atY = startY + gone * sin; if (ArMath::squaredDistanceBetween( atX, atY, robotX, robotY) < maxSquared) myCurrentBuffer.redoReading(atX, atY); } // now check the end point if (end.squaredFindDistanceTo(myRobot->getPose()) < maxSquared) myCurrentBuffer.redoReading(end.getX(), end.getY()); } } myDataMutex.unlock(); // and we're done myCurrentBuffer.endRedoBuffer(); unlockDevice(); //printf("%d\n", startingTime.mSecSince()); }
void addGoalFailed(ArPose pose){ ArLog::log(ArLog::Normal, "Fail at x = %f, y = %f", pose.getX(), pose.getY()); }
void RosArnlNode::publish() { // todo could only publish if robot not stopped (unless arnl has TriggerTime // set in which case it might update localization even ifnot moving), or // use a callback from arnl for robot pose updates rather than every aria // cycle. In particular, getting the covariance is a bit computational // intensive and not everyone needs it. ArTime tasktime; // Note, this is called via SensorInterpTask callback (myPublishCB, named "ROSPublishingTask"). ArRobot object 'robot' sholud not be locked or unlocked. ArPose pos = arnl.robot->getPose(); // convert mm and degrees to position meters and quaternion angle in ros pose tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(pos.getTh()*M_PI/180), tf::Vector3(pos.getX()/1000, pos.getY()/1000, 0)), pose_msg.pose.pose); pose_msg.header.frame_id = "map"; // ARIA/ARNL times are in reference to an arbitrary starting time, not OS // clock, so find the time elapsed between now and last ARNL localization // to adjust the time stamp in ROS time vs. now accordingly. //pose_msg.header.stamp = ros::Time::now(); ArTime loctime = arnl.locTask->getLastLocaTime(); ArTime arianow; const double dtsec = (double) loctime.mSecSince(arianow) / 1000.0; //printf("localization was %f seconds ago\n", dtsec); pose_msg.header.stamp = ros::Time(ros::Time::now().toSec() - dtsec); // TODO if robot is stopped, ARNL won't re-localize (unless TriggerTime option is // configured), so should we just use Time::now() in that case? or do users // expect long ages for poses if robot stopped? #if 0 { printf("ros now is %12d sec + %12d nsec = %f seconds\n", ros::Time::now().sec, ros::Time::now().nsec, ros::Time::now().toSec()); ArTime t; printf("aria now is %12lu sec + %12lu ms\n", t.getSec(), t.getMSec()); printf("arnl loc is %12lu sec + %12lu ms\n", loctime.getSec(), loctime.getMSec()); printf("pose stamp:= %12d sec + %12d nsec = %f seconds\n", pose_msg.header.stamp.sec, pose_msg.header.stamp.nsec, pose_msg.header.stamp.toSec()); double d = ros::Time::now().toSec() - pose_msg.header.stamp.toSec(); printf("diff is %12f sec, \n", d); puts("----"); } #endif #ifndef ROS_ARNL_NO_COVARIANCE ArMatrix var; ArPose meanp; if(arnl.locTask->findLocalizationMeanVar(meanp, var)) { // ROS pose covariance is 6x6 with position and orientation in 3 // dimensions each x, y, z, roll, pitch, yaw (but placed all in one 1-d // boost::array container) // // ARNL has x, y, yaw (aka theta): // 0 1 2 // 0 x*x x*y x*yaw // 1 y*x y*y y*yaw // 2 yaw*x yaw*y yaw*yaw // // Also convert mm to m and degrees to radians. // // all elements in pose_msg.pose.covariance were initialized to -1 (invalid // marker) in the RosArnlNode constructor, so just update elements that // contain x, y and yaw. pose_msg.pose.covariance[6*0 + 0] = var(0,0)/1000.0; // x/x pose_msg.pose.covariance[6*0 + 1] = var(0,1)/1000.0; // x/y pose_msg.pose.covariance[6*0 + 5] = ArMath::degToRad(var(0,2)/1000.0); //x/yaw pose_msg.pose.covariance[6*1 + 0] = var(1,0)/1000.0; //y/x pose_msg.pose.covariance[6*1 + 1] = var(1,1)/1000.0; // y/y pose_msg.pose.covariance[6*1 + 5] = ArMath::degToRad(var(1,2)/1000.0); // y/yaw pose_msg.pose.covariance[6*5 + 0] = ArMath::degToRad(var(2,0)/1000.0); //yaw/x pose_msg.pose.covariance[6*5 + 1] = ArMath::degToRad(var(2,1)/1000.0); // yaw*y pose_msg.pose.covariance[6*5 + 5] = ArMath::degToRad(var(2,2)); // yaw*yaw } #endif pose_pub.publish(pose_msg); if(action_executing) { move_base_msgs::MoveBaseFeedback feedback; feedback.base_position.pose = pose_msg.pose.pose; actionServer.publishFeedback(feedback); } // publishing transform map->base_link map_trans.header.stamp = ros::Time::now(); map_trans.header.frame_id = frame_id_map; map_trans.child_frame_id = frame_id_base_link; map_trans.transform.translation.x = pos.getX()/1000; map_trans.transform.translation.y = pos.getY()/1000; map_trans.transform.translation.z = 0.0; map_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pos.getTh()*M_PI/180); map_broadcaster.sendTransform(map_trans); // publish motors state if changed bool e = arnl.robot->areMotorsEnabled(); if(e != motors_state.data || !published_motors_state) { ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: publishing new motors state: %s.", e?"yes":"no"); motors_state.data = e; motors_state_pub.publish(motors_state); published_motors_state = true; } const char *s = arnl.getServerStatus(); if(s != NULL && lastServerStatus != s) { ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: publishing new server status: %s", s); lastServerStatus = s; std_msgs::String msg; msg.data = lastServerStatus; arnl_server_status_pub.publish(msg); } const char *m = arnl.getServerMode(); if(m != NULL && lastServerMode != m) { ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: publishing now server mode: %s", m); lastServerMode = m; std_msgs::String msg; msg.data = lastServerMode; arnl_server_mode_pub.publish(msg); } ROS_WARN_COND_NAMED((tasktime.mSecSince() > 20), "rosarnl_node", "rosarnl_node: publish aria task took %ld ms", tasktime.mSecSince()); }
void GPSMapTools::addHomeHere(ArArgumentBuilder *args) { if(!checkGPS("adding home")) return; if(!checkMap("adding home")) return; ArPose p = getCurrentPosFromGPS(); ArLog::log(ArLog::Normal, "GPSMapTools: Adding home in map at GPS position (x=%.2f, y=%.2f)", p.getX(), p.getY()); myMap->lock(); ArMapObject *newobj; myMap->getMapObjects()->push_back(newobj = new ArMapObject("Home", p, "Home", "ICON", args->getFullString(), false, ArPose(0, 0), ArPose(0, 0))); printf("\tnew map object is:\n\t%s\n", newobj->toString()); newobj->log(); myMap->writeFile(myMap->getFileName(), true); myMap->unlock(); reloadMapFile(); }
void PRM::render() { //Desenhando o mapa em si glBegin(GL_QUADS); for(int x=0;x<mMapSize;x++) { for(int y=0;y<mMapSize;y++) { if(mMap[x][y] != 1.0) { int value = 255*mMap[x][y]; drawBox( (x-1-mMapSize/2)*celRange, (mMapSize/2 - y+1)*celRange, celRange, celRange, QColor(value,value,value) ); } } } glEnd(); if(mGraph) { //Desenhando os Arcos glColor3f(0.0f,0.0f,1.0f); glBegin(GL_LINES); for(int i =0; i<mGraph->E.size();i++) { ArPose *pL = mGraph->E.at(i)->mNodeL->mPose, *pR = mGraph->E.at(i)->mNodeR->mPose; glVertex2f((pL->getX()-mMapSize/2)*celRange,(mMapSize/2 - pL->getY())*celRange); glVertex2f((pR->getX()-mMapSize/2)*celRange,(mMapSize/2 - pR->getY())*celRange); } glEnd(); //Desenhando os nodos glBegin(GL_QUADS); for(int i =0;i<mGraph->V.size();i++) { ArPose *p = mGraph->V.at(i)->mPose; drawBox( (p->getX() - 1 - mMapSize/2)*celRange, (mMapSize/2 - 1 - p->getY())*celRange, celRange*2, celRange*2, QColor(0,255,255) ); } glEnd(); } if(mInitNode && mGoalNode && path.size()) { //Desenhando os Arcos glColor3f(1.0f,0.0f,1.0f); glLineWidth(3.0f); glBegin(GL_LINES); for(int i =0; i<path.size();i++) { ArPose *pL = path.at(i)->mNodeL->mPose, *pR = path.at(i)->mNodeR->mPose; glVertex2f((pL->getX()-mMapSize/2)*celRange,(mMapSize/2 - pL->getY())*celRange); glVertex2f((pR->getX()-mMapSize/2)*celRange,(mMapSize/2 - pR->getY())*celRange); } glEnd(); glLineWidth(1.0f); } //Desenhando os objetivo e posicao inicial if(mInitNode) { glBegin(GL_QUADS); drawBox( (mInitNode->mPose->getX() - 2 - mMapSize/2)*celRange, (mMapSize/2 - 1 - mInitNode->mPose->getY())*celRange, celRange*3, celRange*3, QColor(0,150,0) ); glEnd(); } if(mGoalNode) { glBegin(GL_QUADS); drawBox( (mGoalNode->mPose->getX() - 2 - mMapSize/2)*celRange, (mMapSize/2 - 1 - mGoalNode->mPose->getY())*celRange, celRange*3, celRange*3, QColor(255,0,0) ); glEnd(); } }
void RosAriaNode::publish() { // Note, this is called via SensorInterpTask callback (myPublishCB, named "ROSPublishingTask"). ArRobot object 'robot' sholud not be locked or unlocked. pos = robot->getPose(); tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(pos.getTh()*M_PI/180), tf::Vector3(pos.getX()/1000, pos.getY()/1000, 0)), position.pose.pose); //Aria returns pose in mm. position.twist.twist.linear.x = robot->getVel()/1000; //Aria returns velocity in mm/s. position.twist.twist.angular.z = robot->getRotVel()*M_PI/180; position.header.frame_id = frame_id_odom; position.child_frame_id = frame_id_base_link; position.header.stamp = ros::Time::now(); pose_pub.publish(position); ROS_DEBUG("RosAria: publish: (time %f) pose x: %f, y: %f, angle: %f; linear vel x: %f, y: %f; angular vel z: %f", position.header.stamp.toSec(), (double)position.pose.pose.position.x, (double)position.pose.pose.position.y, (double)position.pose.pose.orientation.w, (double) position.twist.twist.linear.x, (double) position.twist.twist.linear.y, (double) position.twist.twist.angular.z ); // publishing transform odom->base_link odom_trans.header.stamp = ros::Time::now(); odom_trans.header.frame_id = frame_id_odom; odom_trans.child_frame_id = frame_id_base_link; odom_trans.transform.translation.x = pos.getX()/1000; odom_trans.transform.translation.y = pos.getY()/1000; odom_trans.transform.translation.z = 0.0; odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pos.getTh()*M_PI/180); odom_broadcaster.sendTransform(odom_trans); // getStallValue returns 2 bytes with stall bit and bumper bits, packed as (00 00 FrontBumpers RearBumpers) int stall = robot->getStallValue(); unsigned char front_bumpers = (unsigned char)(stall >> 8); unsigned char rear_bumpers = (unsigned char)(stall); bumpers.header.frame_id = frame_id_bumper; bumpers.header.stamp = ros::Time::now(); std::stringstream bumper_info(std::stringstream::out); // Bit 0 is for stall, next bits are for bumpers (leftmost is LSB) for (unsigned int i=0; i<robot->getNumFrontBumpers(); i++) { bumpers.front_bumpers[i] = (front_bumpers & (1 << (i+1))) == 0 ? 0 : 1; bumper_info << " " << (front_bumpers & (1 << (i+1))); } ROS_DEBUG("RosAria: Front bumpers:%s", bumper_info.str().c_str()); bumper_info.str(""); // Rear bumpers have reverse order (rightmost is LSB) unsigned int numRearBumpers = robot->getNumRearBumpers(); for (unsigned int i=0; i<numRearBumpers; i++) { bumpers.rear_bumpers[i] = (rear_bumpers & (1 << (numRearBumpers-i))) == 0 ? 0 : 1; bumper_info << " " << (rear_bumpers & (1 << (numRearBumpers-i))); } ROS_DEBUG("RosAria: Rear bumpers:%s", bumper_info.str().c_str()); bumpers_pub.publish(bumpers); //Publish battery information // TODO: Decide if BatteryVoltageNow (normalized to (0,12)V) is a better option std_msgs::Float64 batteryVoltage; batteryVoltage.data = robot->getRealBatteryVoltageNow(); voltage_pub.publish(batteryVoltage); if(robot->haveStateOfCharge()) { std_msgs::Float32 soc; soc.data = robot->getStateOfCharge()/100.0; state_of_charge_pub.publish(soc); } // publish recharge state if changed char s = robot->getChargeState(); if(s != recharge_state.data) { ROS_INFO("RosAria: publishing new recharge state %d.", s); recharge_state.data = s; recharge_state_pub.publish(recharge_state); } // publish motors state if changed bool e = robot->areMotorsEnabled(); if(e != motors_state.data || !published_motors_state) { ROS_INFO("RosAria: publishing new motors state %d.", e); motors_state.data = e; motors_state_pub.publish(motors_state); published_motors_state = true; } if (robot->areSonarsEnabled()) { int i = 0; int j = 0; ArSensorReading* reading = NULL; if(sonars__crossed_the_streams) { i = 8; j = 8; } for(; i < 16; i++) { ranges.data[i].header.stamp = ros::Time::now(); ArSensorReading* _reading = NULL; _reading = robot->getSonarReading(i-j); ranges.data[i].range = _reading->getRange() / 1000.0f; range_pub[i].publish(ranges.data[i]); } ranges.header.stamp = ros::Time::now(); combined_range_pub.publish(ranges); } }
/*! * This is the called when path to goal fails * * @param goal: Goal it was assigned. */ void Advanced::goalFailed(ArPose goal) { printf("goalFailed:Main: %5.2f %5.2f %5.2f\07\n", goal.getX(), goal.getY(), goal.getTh()); }
void RosArnlNode::execute_action_cb(const move_base_msgs::MoveBaseGoalConstPtr &goal) { // the action execute callback is initiated by the first goal sent. it should // continue running until the goal is reached or failed. we need to also // check here for new goals received while the previous goal is in progress, // in which case we will set the new goal. arnl callbacks are used to handle // reaching the goal, failure, or recognizing that the goal has been // preempted, which allows it to work in combination with MobileEyes or other // clients as well as the ros action client. ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: action: begin execution for new goal."); action_executing = true; ArPose goalpose = rosPoseToArPose(goal->target_pose); ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: action: planning to goal %.0fmm, %.0fmm, %.0fdeg", goalpose.getX(), goalpose.getY(), goalpose.getTh()); bool heading = !ArMath::isNan(goalpose.getTh()); //arnl.pathTask->pathPlanToPose(goalpose, heading); arnl.modeGoto->gotoPose(goalpose, heading); arnl_goal_done = false; while(n.ok()) { // TODO check for localization lost if(arnl_goal_done) { ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: action: goal done, ending execution."); action_executing = false; return; } if(actionServer.isPreemptRequested()) { if(actionServer.isNewGoalAvailable()) { // we were preempted by a new goal move_base_msgs::MoveBaseGoalConstPtr newgoal = actionServer.acceptNewGoal(); goalpose = rosPoseToArPose(newgoal->target_pose); ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: action: new goal interrupted current goal. planning to new goal %.0fmm, %.0fmm, %.0fdeg", goalpose.getX(), goalpose.getY(), goalpose.getTh()); bool heading = !ArMath::isNan(goalpose.getTh()); arnl.modeGoto->gotoPose(goalpose, heading); // action server will be set to preempted state by arnl interrupt // callback. } else { // we were simply asked to just go to "preempted" end state, with no new // goal ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: action: forced to preempted, ending execution."); actionServer.setPreempted(); action_executing = false; arnl.modeGoto->deactivate(); return; } } // feedback is published in the publish() task callback } // node is shutting down, n.ok() returned false ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: action: node shutting down, setting aborted state and ending execution."); actionServer.setAborted(move_base_msgs::MoveBaseResult(), "Setting aborted state since node is shutting down."); action_executing = false; }
void RosArnlNode::simple_goal_sub_cb(const geometry_msgs::PoseStampedConstPtr &msg) { ArPose p = rosPoseToArPose(msg->pose); bool heading = !ArMath::isNan(p.getTh()); ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Received goal %.0fmm, %.0fmm, %.0fdeg", p.getX(), p.getY(), p.getTh()); //arnl.pathTask->pathPlanToPose(p, true); arnl.modeGoto->gotoPose(p, heading); }
void RosArnlNode::initialpose_sub_cb(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg) { ArPose p = rosPoseToArPose(msg->pose.pose); ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Init localization pose received %.0fmm, %.0fmm, %.0fdeg", p.getX(), p.getY(), p.getTh()); arnl.locTask->forceUpdatePose(p); }
void addGoalInterrupted(ArPose pose) { ArLog::log(ArLog::Normal, "Interrupted at x = %f, y = %f", pose.getX(), pose.getY()); }
void Joydrive::drive(void) { int trans, rot; ArPose pose; ArPose rpose; ArTransform transform; ArRangeDevice *dev; ArSensorReading *son; if (!myRobot->isConnected()) { printf("Lost connection to the robot, exiting\n"); exit(0); } printf("\rx %6.1f y %6.1f th %6.1f", myRobot->getX(), myRobot->getY(), myRobot->getTh()); fflush(stdout); if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(1)) { if (ArMath::fabs(myRobot->getVel()) < 10.0) myRobot->comInt(ArCommands::ENABLE, 1); myJoyHandler.getAdjusted(&rot, &trans); myRobot->setVel(trans); myRobot->setRotVel(-rot); } else { myRobot->setVel(0); myRobot->setRotVel(0); } if (myJoyHandler.haveJoystick() && myJoyHandler.getButton(2) && time(NULL) - myLastPress > 1) { myLastPress = time(NULL); printf("\n"); switch (myTest) { case 1: printf("Moving back to the origin.\n"); pose.setPose(0, 0, 0); myRobot->moveTo(pose); break; case 2: printf("Moving over a meter.\n"); pose.setPose(myRobot->getX() + 1000, myRobot->getY(), 0); myRobot->moveTo(pose); break; case 3: printf("Doing a transform test....\n"); printf("\nOrigin should be transformed to the robots coords.\n"); transform = myRobot->getToGlobalTransform(); pose.setPose(0, 0, 0); pose = transform.doTransform(pose); rpose = myRobot->getPose(); printf("Pos: "); pose.log(); printf("Robot: "); rpose.log(); if (pose.findDistanceTo(rpose) < .1) printf("Success\n"); else printf("#### FAILURE\n"); printf("\nRobot coords should be transformed to the origin.\n"); transform = myRobot->getToLocalTransform(); pose = myRobot->getPose(); pose = transform.doTransform(pose); rpose.setPose(0, 0, 0); printf("Pos: "); pose.log(); printf("Robot: "); rpose.log(); if (pose.findDistanceTo(rpose) < .1) printf("Success\n"); else printf("#### FAILURE\n"); break; case 4: printf("Doing a tranform test...\n"); printf("A point 1 meter to the -x from the robot (in local coords) should be transformed into global coordinates.\n"); transform = myRobot->getToGlobalTransform(); pose.setPose(-1000, 0, 0); pose = transform.doTransform(pose); rpose = myRobot->getPose(); printf("Pos: "); pose.log(); printf("Robot: "); rpose.log(); if (ArMath::fabs(pose.findDistanceTo(rpose) - 1000.0) < .1) printf("Probable Success\n"); else printf("#### FAILURE\n"); break; case 5: printf("Doing a transform test on range devices..\n"); printf("Moving the robot +4 meters x and +4 meters y and seeing if the moveTo will move the sonar readings along with it.\n"); dev = myRobot->findRangeDevice("sonar"); if (dev == NULL) { printf("No sonar on the robot, can't do the test.\n"); break; } printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0)); printf("Sonar 0 reading is at "); son = myRobot->getSonarReading(0); if (son != NULL) { pose = son->getPose(); pose.log(); } pose = myRobot->getPose(); pose.setX(pose.getX() + 4000); pose.setY(pose.getY() + 4000); myRobot->moveTo(pose); printf("Moved robot.\n"); printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0)); printf("Sonar 0 reading is at "); son = myRobot->getSonarReading(0); if (son != NULL) { pose = son->getPose(); pose.log(); } break; case 6: printf("Robot position now is:\n"); pose = myRobot->getPose(); pose.log(); printf("Disconnecting from the robot, then reconnecting.\n"); myRobot->disconnect(); myRobot->blockingConnect(); printf("Robot position now is:\n"); pose = myRobot->getPose(); pose.log(); break; default: printf("No test for second button.\n"); break; } } }
void addGoalDone(ArPose pose) { ArLog::log(ArLog::Normal, "x = %f, y = %f", pose.getX(), pose.getY()); }
int main(int argc, char **argv) { Aria::init(); ArClientBase client; ArArgumentParser parser(&argc, argv); /* This will be used to connect our client to the server. * It will get the hostname from the -host command line argument: */ ArClientSimpleConnector clientConnector(&parser); parser.loadDefaultArguments(); /* Check for -host, -help, ARIA arguments, and unhandled arguments: */ if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed()) { Aria::logOptions(); Aria::exit(0); } /* Connect our client object to the remote server: */ if (!clientConnector.connectClient(&client)) { if (client.wasRejected()) printf("Server '%s' rejected connection, exiting\n", client.getHost()); else printf("Could not connect to server '%s', exiting\n", client.getHost()); Aria::exit(1); } printf("Connected to server.\n"); client.setRobotName(client.getHost()); // include server name in log messages client.runAsync(); ArNetPacket request; // request. // client.requestOnceByCommand(ArCommands::ENABLE, ) ArClientHandlerRobotUpdate updates(&client); // client.requestOnce("enableMotor"); ArGlobalFunctor1<ArNetPacket*> enableCB(&enable); ArGlobalFunctor1<ArNetPacket *> getFileCB(&netGetFile); ArGlobalFunctor1<ArNetPacket*> recieveDataCB(&recieveData); client.addHandler("requestEnableMotor", &enableCB); client.addHandler("handleCheckObjectData", &recieveDataCB); //client.addHandler("getFile", &getFileCB); client.requestOnce("requestEnableMotor"); client.request("handleCheckObjectData", 10); updates.requestUpdates(); if (checkObject) client.requestOnceWithString("getFile", "./image/ball.jpg"); ArPose pose; namedWindow("image", 0); while (client.getRunningWithLock()) { //client.requestOnce("sendData"); if (checkObject) { // if (!a) { cout <<"OK"<<endl; // client.requestOnceWithString("getFile", "./image/ball.jpg"); client.addHandler("getFile", &getFileCB); // client.remHandler("getFile", &getFileCB); //} else { //client.remHandler("getFile", &getFileCB); //} Mat image; image = imread("./image/ball.jpg", CV_LOAD_IMAGE_COLOR); imshow("image", image); char c = (char)waitKey(10); if( c == 27 ) break; } ArUtil::sleep(200); } // client.requestStop("getFile"); cout <<"Da tim thay qua bong o vi tri pose("<<pose.getX()<<", "<<pose.getY()<<")"<<endl; cout <<"Vi tri pose("<<pose.getX()<<", "<<pose.getY()<<")"<<endl; /* The client stopped running, due to disconnection from the server, general * Aria shutdown, or some other reason. */ client.disconnect(); Aria::exit(0); return 0; }
/** This function is called every 100 milliseconds. */ AREXPORT void ArIRs::processReadings(void) { ArUtil::BITS bit; if(myParams.haveTableSensingIR()) { for (int i = 0; i < myParams.getNumIR(); ++i) { switch(i) { case 0: bit = ArUtil::BIT0; break; case 1: bit = ArUtil::BIT1; break; case 2: bit = ArUtil::BIT2; break; case 3: bit = ArUtil::BIT3; break; case 4: bit = ArUtil::BIT4; break; case 5: bit = ArUtil::BIT5; break; case 6: bit = ArUtil::BIT6; break; case 7: bit = ArUtil::BIT7; break; } if(myParams.haveNewTableSensingIR() && myRobot->getIODigInSize() > 3) { if((myParams.getIRType(i) && !(myRobot->getIODigIn(3) & bit)) || (!myParams.getIRType(i) && (myRobot->getIODigIn(3) & bit))) { if(cycleCounters[i] < myParams.getIRCycles(i)) { cycleCounters[i] = cycleCounters[i] + 1; } else { cycleCounters[i] = 1; ArPose pose; pose.setX(myParams.getIRX(i)); pose.setY(myParams.getIRY(i)); ArTransform global = myRobot->getToGlobalTransform(); pose = global.doTransform(pose); myCurrentBuffer.addReading(pose.getX(), pose.getY()); } } else { cycleCounters[i] = 1; } } else { if(!(myRobot->getDigIn() & bit)) { if(cycleCounters[i] < myParams.getIRCycles(i)) { cycleCounters[i] = cycleCounters[i] + 1; } else { cycleCounters[i] = 1; ArPose pose; pose.setX(myParams.getIRX(i)); pose.setY(myParams.getIRY(i)); ArTransform global = myRobot->getToGlobalTransform(); pose = global.doTransform(pose); myCurrentBuffer.addReading(pose.getX(), pose.getY()); } } else { cycleCounters[i] = 1; } } } } }
void readPosition(ArRobot& robot){ ArPose pose = robot.getPose(); fseek(G_pose_fd, SEEK_SET, 0); fprintf(G_pose_fd, "x=%0.6f;y=%0.6f;th=%0.6f;\n", pose.getX(), pose.getY(), pose.getTh()); }