/** * @brief publish sonar readings */ void AriaCore::publish_sonar() { sensor_msgs::PointCloud msg; msg.header.stamp = ros::Time::now(); msg.header.frame_id = m_frameIDSonar; m_robot.lock(); { int i; ArSensorReading* pRead; for (i=0; i<m_robot.getNumSonar(); ++i) { pRead = m_robot.getSonarReading(i); if (!pRead) { ROS_WARN("Did not receive a sonar reading on %d", i); continue; } geometry_msgs::Point32 point; point.x = pRead->getLocalX(); point.y = pRead->getLocalY(); point.z = 0.0; msg.points.push_back(point); } } m_robot.unlock(); m_pubSonar.publish(msg); }
void ArSickLogger::internalTakeReading(void) { const std::list<ArSensorReading *> *readings; std::list<ArSensorReading *>::const_iterator it; std::list<ArSensorReading *>::const_reverse_iterator rit; ArPose poseTaken; time_t msec; ArSensorReading *reading; bool usingAdjustedReadings; // we take readings in any of the following cases if we haven't // taken one yet or if we've been explicitly told to take one or if // we've gone further than myDistDiff if we've turned more than // myDegDiff if we've switched sign on velocity and gone more than // 50 mm (so it doesn't oscilate and cause us to trigger) if (myRobot->isConnected() && (!myFirstTaken || myTakeReadingExplicit || myLast.findDistanceTo(myRobot->getEncoderPose()) > myDistDiff || fabs(ArMath::subAngle(myLast.getTh(), myRobot->getEncoderPose().getTh())) > myDegDiff || (( (myLastVel < 0 && myRobot->getVel() > 0) || (myLastVel > 0 && myRobot->getVel() < 0)) && myLast.findDistanceTo(myRobot->getEncoderPose()) > 50))) { myWrote = true; mySick->lockDevice(); /// use the adjusted raw readings if we can, otherwise just use /// the raw readings like before if ((readings = mySick->getAdjustedRawReadings()) != NULL) { usingAdjustedReadings = true; } else { usingAdjustedReadings = false; readings = mySick->getRawReadings(); } if (readings == NULL || (it = readings->begin()) == readings->end() || myFile == NULL) { mySick->unlockDevice(); return; } myTakeReadingExplicit = false; myScanNumber++; if (usingAdjustedReadings) ArLog::log(ArLog::Normal, "Taking adjusted readings from the %d laser values", readings->size()); else ArLog::log(ArLog::Normal, "Taking readings from the %d laser values", readings->size()); myFirstTaken = true; myLast = myRobot->getEncoderPose(); poseTaken = (*readings->begin())->getEncoderPoseTaken(); myLastVel = myRobot->getVel(); msec = myStartTime.mSecSince(); fprintf(myFile, "scan1Id: %d\n", myScanNumber); fprintf(myFile, "time: %ld.%ld\n", msec / 1000, msec % 1000); /* ScanStudio isn't using these yet so don't print them fprintf(myFile, "velocities: %.2f %.2f\n", myRobot->getRotVel(), myRobot->getVel());*/ internalPrintPos(poseTaken); if (myUseReflectorValues) { fprintf(myFile, "reflector1: "); if (!mySick->isLaserFlipped()) { // make sure that the list is in increasing order for (it = readings->begin(); it != readings->end(); it++) { reading = (*it); if (!reading->getIgnoreThisReading()) fprintf(myFile, "%d ", reading->getExtraInt()); else fprintf(myFile, "0 "); } } else { for (rit = readings->rbegin(); rit != readings->rend(); rit++) { reading = (*rit); if (!reading->getIgnoreThisReading()) fprintf(myFile, "%d ", reading->getExtraInt()); else fprintf(myFile, "0 "); } } fprintf(myFile, "\n"); } /** Note that the the sick1: or scan1: must be the last thing in that timestamp, ie that you should put any other data before it. **/ if (myOldReadings) { fprintf(myFile, "sick1: "); if (!mySick->isLaserFlipped()) { // make sure that the list is in increasing order for (it = readings->begin(); it != readings->end(); it++) { reading = (*it); fprintf(myFile, "%d ", reading->getRange()); } } else { for (rit = readings->rbegin(); rit != readings->rend(); rit++) { reading = (*rit); fprintf(myFile, "%d ", reading->getRange()); } } fprintf(myFile, "\n"); } if (myNewReadings) { fprintf(myFile, "scan1: "); if (!mySick->isLaserFlipped()) { // make sure that the list is in increasing order for (it = readings->begin(); it != readings->end(); it++) { reading = (*it); if (!reading->getIgnoreThisReading()) fprintf(myFile, "%.0f %.0f ", reading->getLocalX() - mySick->getSensorPositionX(), reading->getLocalY() - mySick->getSensorPositionY()); else fprintf(myFile, "0 0 "); } } else { for (rit = readings->rbegin(); rit != readings->rend(); rit++) { reading = (*rit); if (!reading->getIgnoreThisReading()) fprintf(myFile, "%.0f %.0f ", reading->getLocalX() - mySick->getSensorPositionX(), reading->getLocalY() - mySick->getSensorPositionY()); else fprintf(myFile, "0 0 "); } } fprintf(myFile, "\n"); } mySick->unlockDevice(); } }
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); } }