/**
* @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);
}
}
