int main(int argc, char **argv){
Aria::init();
ArRobot robot;
ArArgumentParser parser(&argc, argv);
ArSimpleConnector connector(& parser);
parser.loadDefaultArguments();
Aria::logOptions();
if (!connector.parseArgs()){
cout << "Unknown settings\n";
Aria::exit(0);
exit(1);
}
if (!connector.connectRobot(&robot)){
cout << "Unable to connect\n";
Aria::exit(0);
exit(1);
}
robot.runAsync(true);
robot.lock();
robot.comInt(ArCommands::ENABLE, 1);
robot.unlock();
ArSonarDevice sonar;
robot.addRangeDevice(&sonar);
G_id = 0;
G_SONAR_FD = fopen("../sensors/sonars","w");
G_pose_fd = fopen("../sensors/pose","w");
int numSonar = robot.getNumSonar();
while(1){
readPosition(robot);
readSonars(robot, 8);
setMotors(robot);
usleep(20000);
}
fclose(G_SONAR_FD);
fclose(G_pose_fd);
Aria::exit(0);
}
ArActionDesired* ActionReadSonar::fire(ArActionDesired currentDesired)
{
ArRobot *robot = this->getRobot();
int total = robot->getNumSonar(); // get the total number of sonar on the robot
ArSensorReading* value; // This class abstracts range and angle read from sonar
//cout << " 0 : " << robot->getSonarReading(0)->getSensorTh() << " 1 : " << robot->getSonarReading(1)->getSensorTh()
// << " 2 : " << robot->getSonarReading(2)->getSensorTh() << " 3 : " << robot->getSonarReading(3)->getSensorTh()
// << " 4 : " << robot->getSonarReading(4)->getSensorTh() << " 5 : " << robot->getSonarReading(5)->getSensorTh()
// << " 6 : " << robot->getSonarReading(6)->getSensorTh() << " 7 : " << robot->getSonarReading(7)->getSensorTh()
// << "r :" << robot->getTh() << endl;
double limit = 800;
double distance;
// reset the actionDesired (must be done), to clear
// its previous values.
myDesired.reset();
// if the sonar is null we can't do anything, so deactivate
if (mySonar == NULL)
{
deactivate();
return NULL;
}
// gets value of object between -20 degrees and 20 degrees of foward
double angle = 0;
distance = mySonar->currentReadingPolar(-20, 20, &angle);
//cout << "distance from nearest object =" << distance << endl;
if (distance <= limit) {
int heading = 15;
//cout << "angle :" << angle << endl;
if (angle > 10) {
heading = -heading;
}
else if (angle < -10) {
heading = heading;
}
//cout << "x" << robot->getX() << " ," << robot->getY() << endl;
cout << "distance from nearest object =" << distance << endl;
robot->lock();
robot->setVel(0);
robot->unlock();
robot->lock();
robot->setDeltaHeading(heading);
robot->unlock();
ArUtil::sleep(50);
}
return &myDesired;
}
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);
}
}
