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)
{
bool done;
double distToTravel = 2300;
// whether to use the sim for the laser or not, if you use the sim
// for hte laser, you have to use the sim for the robot too
bool useSim = false;
// the laser
ArSick sick;
// connection
ArDeviceConnection *con;
// Laser connection
ArSerialConnection laserCon;
// robot
ArRobot robot;
// set a default filename
//std::string filename = "c:\\log\\1scans.2d";
std::string filename = "1scans.2d";
// see if we want to use a different filename
//if (argc > 1)
//Lfilename = argv[1];
printf("Logging to file %s\n", filename.c_str());
// start the logger with good values
sick.configureShort(useSim, ArSick::BAUD38400,
ArSick::DEGREES180, ArSick::INCREMENT_HALF);
ArSickLogger logger(&robot, &sick, 300, 25, filename.c_str());
// mandatory init
Aria::init();
// add it to the robot
robot.addRangeDevice(&sick);
//ArAnalogGyro gyro(&robot);
// if we're not using the sim, make a serial connection and set it up
if (!useSim)
{
ArSerialConnection *serCon;
serCon = new ArSerialConnection;
serCon->setPort();
//serCon->setBaud(38400);
con = serCon;
}
// if we are using the sim, set up a tcp connection
else
{
ArTcpConnection *tcpCon;
tcpCon = new ArTcpConnection;
tcpCon->setPort();
con = tcpCon;
}
// set the connection on the robot
robot.setDeviceConnection(con);
// try to connect, if we fail exit
if (!robot.blockingConnect())
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
// set up a key handler so escape exits and attach to the robot
ArKeyHandler keyHandler;
robot.attachKeyHandler(&keyHandler);
// run the robot, true here so that the run will exit if connection lost
robot.runAsync(true);
// if we're not using the sim, set up the port for the laser
if (!useSim)
{
laserCon.setPort(ArUtil::COM3);
sick.setDeviceConnection(&laserCon);
}
// now that we're connected to the robot, connect to the laser
sick.runAsync();
if (!sick.blockingConnect())
{
printf("Could not connect to SICK laser... exiting\n");
robot.disconnect();
Aria::shutdown();
return 1;
}
#ifdef WIN32
// wait until someone pushes the motor button to go
while (1)
{
robot.lock();
if (!robot.isRunning())
exit(0);
if (robot.areMotorsEnabled())
{
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(100);
}
#endif
// basically from here on down the robot just cruises around a bit
robot.lock();
// enable the motors, disable amigobot sounds
robot.comInt(ArCommands::ENABLE, 1);
ArTime startTime;
// move a couple meters
robot.move(distToTravel);
robot.unlock();
startTime.setToNow();
do {
ArUtil::sleep(100);
robot.lock();
robot.setHeading(0);
done = robot.isMoveDone(60);
robot.unlock();
} while (!done);
/*
// rotate a few times
robot.lock();
robot.setVel(0);
robot.setRotVel(60);
robot.unlock();
ArUtil::sleep(12000);
*/
robot.lock();
robot.setHeading(180);
robot.unlock();
do {
ArUtil::sleep(100);
robot.lock();
robot.setHeading(180);
done = robot.isHeadingDone();
robot.unlock();
} while (!done);
// move a couple meters
robot.lock();
robot.move(distToTravel);
robot.unlock();
startTime.setToNow();
do {
ArUtil::sleep(100);
robot.lock();
robot.setHeading(180);
done = robot.isMoveDone(60);
robot.unlock();
} while (!done);
robot.lock();
robot.setHeading(0);
robot.setVel(0);
robot.unlock();
startTime.setToNow();
do {
ArUtil::sleep(100);
robot.lock();
robot.setHeading(0);
done = robot.isHeadingDone();
robot.unlock();
} while (!done);
sick.lockDevice();
sick.disconnect();
sick.unlockDevice();
robot.lock();
robot.disconnect();
robot.unlock();
// now exit
Aria::shutdown();
return 0;
}
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);
}
}