{

public:

RosArnlNode(ros::NodeHandle n, ArnlSystem& arnlsys);

virtual ~RosArnlNode();

public:

int Setup();

void spin();

void publish();

protected:

ros::NodeHandle n;

ArnlSystem &arnl;

ArFunctorC<RosArnlNode> myPublishCB;

ArPose rosPoseToArPose(const geometry_msgs::Pose& p);

ArPose rosPoseToArPose(const geometry_msgs::PoseStamped& p) { return rosPoseToArPose(p.pose); }

ArPoseWithTime rosPoseStampedToArPoseWithTime(const geometry_msgs::PoseStamped& p);

geometry_msgs::Pose rosPoseToArPose(const ArPose& arpose);

ros::ServiceServer enable_srv;

ros::ServiceServer disable_srv;

ros::ServiceServer wander_srv;

ros::ServiceServer stop_srv;

ros::ServiceServer dock_srv;

bool enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);

bool disable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);

bool wander_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);

bool stop_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);

bool dock_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);

ros::Publisher motors_state_pub;

std_msgs::Bool motors_state;

bool published_motors_state;

class JogPositionActionServer {

public:

ros::NodeHandle nh;

geometry_msgs::Pose2D goal;

geometry_msgs::Pose2D feedback;

ArServerModeJogPosition *jogMode;

ArRobot *robot;

bool executing;

actionlib::SimpleActionServer<rosarnl::JogPositionAction> action_server;

JogPositionActionServer(ros::NodeHandle& _nh, ArServerModeJogPosition *jm, ArRobot *r) :

nh(_nh),

jogMode(jm),

robot(r),

executing(false),

action_server(nh, "jog_position", boost::bind(&JogPositionActionServer::execute, this, _1), false)

{

}

void start() {

action_server.start();

}

// TODO may need to move this to a SimpleActionServer goal callback

// instead of execute, or block at end until ArServerModeJogPosition finishes a

// goal. This may let us update goal status from the main thread rather

// than encountering actionlib errors when this function returns with the

// goal still incomplete.

void execute(const rosarnl::JogPositionGoalConstPtr &goal) {

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Executing new Jog Position action goal (%f, %f, %f)\n", goal->offset.x, goal->offset.y, goal->offset.theta);

bool executingMove = false;

bool executingTurn = false;

if(fabs(goal->offset.x) > 0.000001)

{

jogMode->activate(); // XXX temporary should be automatic in future

jogMode->move( goal->offset.x * 1000.0);

executingMove = true;

}

if(fabs(goal->offset.theta) > 0.000001)

{

jogMode->activate(); // XXX temporary should be automatic in future

jogMode->turn( ArMath::radToDeg(goal->offset.theta) );

executingTurn = true;

}

executing = true;

// determine what the eventual heading should be of the robot after any

// rotation offset is achieved so we can check if its reached yet:

// todo could move that check into ArServerModeJogPosition API

robot->lock();

const double goalHeadingDeg = ArMath::addAngle(robot->getTh(), ArMath::radToDeg(goal->offset.theta));

robot->unlock();

ros::Time startTime = ros::Time::now();

while(executing)

{

if(!action_server.isActive())

{

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Jog Position action server no longer active!");

action_server.setAborted(rosarnl::JogPositionResult(), "Jog Position action server goal cancelled");

}

// todo better to use a deactivate callback instead of calling

// isActive() here?

if(!jogMode->isActive())

{

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Jog Position mode deactivated");

action_server.setAborted(rosarnl::JogPositionResult(), "Jog Position Mode Deactivated");

return;

}

if(action_server.isPreemptRequested())

{

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Jog Position action preempted");

if(action_server.isNewGoalAvailable())

{

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Jog Position action has a new goal after pre-emption. Executing that.");

execute(action_server.acceptNewGoal()); // !! recursive. should we be checking this outside the action thread?

}

else

{

executing = false;

action_server.setPreempted(rosarnl::JogPositionResult(), "Jog Position Preempted");

}

return;

}

// check for jog movements being done

robot->lock();

const double robotTheta = robot->getTh();

robot->unlock();

if( (!executingMove || jogMode->getDriveAction()->haveAchievedDistance()) && (!executingTurn || ArMath::subAngle(goalHeadingDeg, robotTheta) < 0.0001) )

{

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Jog Position action goal done");

executing = false;

action_server.setSucceeded(rosarnl::JogPositionResult(), "Jog Position Done");

return;

}

// check timeout

if(ros::Time::now() - startTime > ros::Duration(goal->timeout))

{

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Jog Position action timed out");

action_server.setAborted(rosarnl::JogPositionResult(), "Jog Position Action Timed Out");

return;

}

// todo publish feedback

// action_server.publishFeedback();

ArUtil::sleep(100);

}

// end of execute()

}

};

JogPositionActionServer jog_position_action_server;

ros::Subscriber simple_jog_position_sub;

void simple_jog_position_sub_cb(const geometry_msgs::Pose2DConstPtr &msg);

geometry_msgs::PoseWithCovarianceStamped pose_msg;

ros::Publisher pose_pub;

ros::Publisher bumpers_pub;

typedef unsigned char bumper_mask_t;

bumper_mask_t front_bumpers, rear_bumpers;

// Battery publishing

ros::Publisher battery_pub;

ros::Time last_battery_pub_time;

ros::Duration battery_pub_period;

geometry_msgs::TransformStamped map_trans;

tf::TransformBroadcaster map_broadcaster;

std::string tf_prefix;

std::string frame_id_map;

std::string frame_id_base_link;

std::string frame_id_bumper;

std::string frame_id_sonar;

ros::Subscriber initialpose_sub;

void initialpose_sub_cb(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg);

ros::ServiceServer global_localization_srv;

bool global_localization_srv_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);

ros::Publisher arnl_server_mode_pub;

ros::Publisher arnl_server_status_pub;

ros::Publisher arnl_path_state_pub;

void arnl_path_state_change_cb();

std::string lastServerStatus;

std::string lastServerMode;

// just request a goal, no actionlib interface:

ros::Subscriber simple_goal_sub;

void simple_goal_sub_cb(const geometry_msgs::PoseStampedConstPtr &msg);

// request goal by name

ros::Subscriber goalname_sub;

void goalname_sub_cb(const std_msgs::StringConstPtr &msg);

ros::Publisher current_goal_pub;

void arnl_new_goal_cb(ArPose p);

// TODO may replace with MoveBase action definitions?

typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> ArnlActionServer;

ArnlActionServer actionServer;

void execute_action_cb(const move_base_msgs::MoveBaseGoalConstPtr& goal);

bool arnl_goal_done;

bool action_executing;

void arnl_goal_reached_cb(ArPose p);

void arnl_goal_failed_cb(ArPose p);

void arnl_goal_interrupted_cb(ArPose p);

// If robot has e--top button pressed, print a warning and return true. Otherwise return false.

bool check_estop(const char *s) {

arnl.robot->lock();

bool e = arnl.robot->isEStopPressed();

arnl.robot->unlock();

if(e) ROS_WARN_NAMED("rosarnl_node", "rosarnl_node: Warning: Robot e-stop button pressed, cannot %s", s);

return e;

}

arnl(arnlsys),

myPublishCB(this, &RosArnlNode::publish),

jog_position_action_server(nh, arnl.modeJogPosition, arnlsys.robot),

actionServer(nh, "move_base", boost::bind(&RosArnlNode::execute_action_cb, this, _1), false),

arnl_goal_done(false),

action_executing(false)

{

n = nh;

// Figure out what frame_id's to use. if a tf_prefix param is specified,

// it will be added to the beginning of the frame_ids.

//

// e.g. rosrun ... _tf_prefix:=MyRobot (or equivalently using <param>s in

// roslaunch files)

// will result in the frame_ids being set to /MyRobot/map etc,

// rather than /map. This is useful for Multi Robot Systems.

// See ROS Wiki for further details.

tf_prefix = tf::getPrefixParam(n);

frame_id_map = tf::resolve(tf_prefix, "map");

frame_id_base_link = tf::resolve(tf_prefix, "base_link");

frame_id_bumper = tf::resolve(tf_prefix, "bumpers_frame");

frame_id_sonar = tf::resolve(tf_prefix, "sonar_frame");

// initialize to all invalid

pose_msg.pose.covariance.assign(-1);

motors_state_pub = n.advertise<std_msgs::Bool>("motors_state", 1, true /*latch*/ );

motors_state.data = false;

published_motors_state = false;

pose_pub = n.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 5, true /*latch*/);

enable_srv = n.advertiseService("enable_motors", &RosArnlNode::enable_motors_cb, this);

disable_srv = n.advertiseService("disable_motors", &RosArnlNode::disable_motors_cb, this);

wander_srv = n.advertiseService("wander", &RosArnlNode::wander_cb, this);

stop_srv = n.advertiseService("stop", &RosArnlNode::stop_cb, this);

dock_srv = n.advertiseService("dock", &RosArnlNode::dock_cb, this);

global_localization_srv = n.advertiseService("global_localization", &RosArnlNode::global_localization_srv_cb, this);

initialpose_sub = n.subscribe("initialpose", 1, (boost::function <void(const geometry_msgs::PoseWithCovarianceStampedConstPtr&)>) boost::bind(&RosArnlNode::initialpose_sub_cb, this, _1));

arnl_server_mode_pub = n.advertise<std_msgs::String>("arnl_server_mode", -1);

arnl_server_status_pub = n.advertise<std_msgs::String>("arnl_server_status", -1);

arnl_path_state_pub = n.advertise<std_msgs::String>("arnl_path_state", -1);

bumpers_pub = n.advertise<rosarnl::BumperState>("bumper_state", 1, true); // latched

front_bumpers = 0xFF; // Initialize to this so we will definitely publish on the first loop

rear_bumpers = 0xFF;

// Battery Publishing

battery_pub = n.advertise<rosarnl::BatteryStatus>("battery_status", 1, true);

battery_pub_period = ros::Duration(5.0);

// TODO the move_base and move_bas_simple topics should be in separate node

// handles?

simple_goal_sub = n.subscribe("move_base_simple/goal", 1, (boost::function <void(const geometry_msgs::PoseStampedConstPtr&)>) boost::bind(&RosArnlNode::simple_goal_sub_cb, this, _1));

goalname_sub = n.subscribe("goalname", 1, (boost::function <void(const std_msgs::StringConstPtr&)>) boost::bind(&RosArnlNode::goalname_sub_cb, this, _1));

current_goal_pub = n.advertise<geometry_msgs::Pose>("current_goal", 1, true);

arnl.pathTask->addNewGoalCB(new ArFunctor1C<RosArnlNode, ArPose>(this, &RosArnlNode::arnl_new_goal_cb));

arnl.pathTask->addGoalFailedCB(new ArFunctor1C<RosArnlNode, ArPose>(this, &RosArnlNode::arnl_goal_failed_cb));

arnl.pathTask->addGoalDoneCB(new ArFunctor1C<RosArnlNode, ArPose>(this, &RosArnlNode::arnl_goal_reached_cb));

arnl.pathTask->addGoalInterruptedCB(new ArFunctor1C<RosArnlNode, ArPose>(this, &RosArnlNode::arnl_goal_interrupted_cb));

arnl.pathTask->addStateChangeCB(new ArFunctorC<RosArnlNode>(this, &RosArnlNode::arnl_path_state_change_cb));

simple_jog_position_sub = n.subscribe("jog_position_simple/goal", 1, (boost::function <void(const geometry_msgs::Pose2DConstPtr&)>) boost::bind(&RosArnlNode::simple_jog_position_sub_cb, this, _1));

// Publish data triggered by ARIA sensor interpretation task

arnl.robot->lock();

arnl.robot->addSensorInterpTask("ROSPublishingTask", 100, &myPublishCB);

arnl.robot->unlock();

{

actionServer.start();

jog_position_action_server.start();

return 0;

{

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Running ROS node...");

ros::spin();

{

// 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.

ros::Time current_time = ros::Time::now();

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);

// Battery info publishing

// Check for periodic timeout

if (current_time - last_battery_pub_time > battery_pub_period)

{

rosarnl::BatteryStatus battery_msg;

battery_msg.charge_percent = arnl.robot->getStateOfCharge();

battery_msg.charging_state = arnl.robot->getChargeState();

battery_pub.publish(battery_msg);

last_battery_pub_time = current_time;

}

// Check if a bumper event has occurred.

const uint16_t stall = arnl.robot->getStallValue();

bumper_mask_t new_front_bumpers = (bumper_mask_t)(stall >> 8); // First byte

bumper_mask_t new_rear_bumpers = (bumper_mask_t)(stall); // Second byte

// Build message and publish if the state has changed

if (new_front_bumpers != front_bumpers || new_rear_bumpers != rear_bumpers) {

front_bumpers = new_front_bumpers;

rear_bumpers = new_rear_bumpers;

rosarnl::BumperState msg;

msg.header.frame_id = frame_id_bumper;

msg.header.stamp = ros::Time::now();

msg.front_bumpers.resize(arnl.robot->getNumFrontBumpers());

msg.rear_bumpers.resize(arnl.robot->getNumRearBumpers());

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, size = arnl.robot->getNumFrontBumpers(); i < size; ++i) {

msg.front_bumpers[i] = (front_bumpers & (1 << (i+1)));

bumper_info << " " << msg.front_bumpers[i];

}

ROS_DEBUG("RosAria: Front bumpers:%s", bumper_info.str().c_str());

bumper_info.str("");

// Rear bumpers have reverse order (rightmost is LSB)

for (unsigned int i=0, size = arnl.robot->getNumRearBumpers(); i < size; ++i) {

msg.rear_bumpers[i] = (rear_bumpers & (1 << (size-i)));

bumper_info << " " << msg.rear_bumpers[i];

}

ROS_DEBUG("RosAria: Rear bumpers:%s", bumper_info.str().c_str());

bumpers_pub.publish(msg);

}

if(action_executing)

{

move_base_msgs::MoveBaseFeedback feedback;

feedback.base_position.pose = pose_msg.pose.pose;

actionServer.publishFeedback(feedback);

}

// jog_position_action_server.check();

// 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());

{

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Enable motors request.");

arnl.robot->lock();

check_estop("enable motors");

arnl.robot->enableMotors();

arnl.robot->unlock();

// todo could wait and see if motors do become enabled, and send a response with an error flag if not

return true;

{

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Disable motors request.");

arnl.robot->lock();

arnl.robot->disableMotors();

arnl.robot->unlock();

// todo could wait and see if motors do become disabled, and send a response with an error flag if not

return true;

{

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Enable wander mode request.");

if(check_estop("enter wander mode"))

return false;

arnl.modeWander->activate();

return true;

{

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Stop request.");

arnl.modeStop->activate();

return true;

{

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Docking procedure request.");

if(check_estop("dock"))

return false;

arnl.modeDock->dock();

return true;

{

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Localize init (global_localization service) request...");

// arnl.locTask->localizeRobotInMapInit();

if(! arnl.locTask->localizeRobotAtHomeBlocking() )

ROS_WARN_NAMED("rosarnl_node", "rosarnl_node: Error in initial localization.");

return true;

{

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: ARNL path planning task state changed to %s", arnl.getPathStateName());

std_msgs::String msg;

msg.data = arnl.getPathStateName();

arnl_path_state_pub.publish(msg);

{

// TODO use tf::poseTFToMsg instead?

geometry_msgs::Pose rospose;

rospose.position.x = arpose.getX() / 1000.0;

rospose.position.y = arpose.getY() / 1000.0;

rospose.position.z = 0;

tf::quaternionTFToMsg(tf::createQuaternionFromYaw(arpose.getTh()*M_PI/180.0), rospose.orientation);

return rospose;

{

ArPoseWithTime arp(rosPoseToArPose(p));

ArTime t;

t.setSec(p.header.stamp.sec);

t.setMSec(p.header.stamp.nsec * 1.0e-6);

arp.setTime(t);

return arp;

{

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);

{

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);

{

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Received named goal \"%s\"", msg->data.c_str());

//arnl.pathTask->pathPlanToGoal(msg->data.c_str());

arnl.modeGoto->gotoGoal(msg->data.c_str());

{

// TODO is it ok to call publish from another thread? (this is called by path

// planning task thread)

// TODO should we start executing action if not yet executing?

current_goal_pub.publish(arPoseToRosPose(arpose));

{

// 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;

{

if(action_executing)

{

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: action: goal succeeded");

actionServer.setSucceeded(move_base_msgs::MoveBaseResult(), "Goal succeeded");

}

else

puts("action not executing");

arnl_goal_done = true;

{

if(action_executing)

{

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: action: goal failed");

actionServer.setAborted(move_base_msgs::MoveBaseResult(), "Goal failed");

}

arnl_goal_done = true;

{

if(action_executing)

{

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: action: goal interrupted");

actionServer.setPreempted();

}

{

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Simple Jog Position goal received on jog_position_simple/goal topic: (%f, %f, %f)\n", msg->x, msg->y, msg->theta);

if(fabs(msg->x) > 0.00001)

{

arnl.modeJogPosition->activate(); // XXX temporary should be automatic in future

arnl.modeJogPosition->move(msg->x * 1000.0);

}

if(fabs(msg->theta) > 0.0001)

{

arnl.modeJogPosition->activate(); // XXX temporary should be automatic in future

arnl.modeJogPosition->turn(ArMath::radToDeg(msg->theta));

}

{

// node that ARIA logging is normally limited at Normal and Terse only. Set

// ARLOG_LEVEL environment variable to override.

switch(level)

{

case ArLog::Normal:

ROS_INFO_NAMED("ARNL", "ARNL: %s", msg);

return;

case ArLog::Terse:

ROS_WARN_NAMED("ARNL", "ARNL: %s", msg);

return;

case ArLog::Verbose:

ROS_DEBUG_NAMED("ARNL", "ARNL: %s", msg);

return;

}

{

ros::init(argc,argv, "rosarnl_node");

Aria::init();

Arnl::init();

/* set log type to None to only use

ariaLogHandler to redirect ARNL log messages to rosconsole by deufault. This can

be changed in the ARNL parameter file however.

*/

ArLog::init(ArLog::None, ArLog::Normal);

ArLog::setFunctor(new ArGlobalFunctor2<const char *, ArLog::LogLevel>(&ariaLogHandler));

ArnlSystem arnl;

if( arnl.setup() != ArnlSystem::OK)

{

ROS_FATAL_NAMED("rosarnl_node", "rosarnl_node: ARNL and ARIA setup failed... \n" );

return -2;

}

ArGlobalFunctor1<int> *ariaExitF = new ArGlobalFunctor1<int>(&Aria::exit, 9);

arnl.robot->addDisconnectOnErrorCB(ariaExitF);

for(std::map<int, ArLaser*>::iterator i = arnl.robot->getLaserMap()->begin();

i != arnl.robot->getLaserMap()->end();

++i)

{

(*i).second->addDisconnectOnErrorCB(ariaExitF);

}

ros::NodeHandle n(std::string("~"));

RosArnlNode *node = new RosArnlNode(n, arnl);

if( node->Setup() != 0 )

{

ROS_FATAL_NAMED("rosarnl_node", "rosarnl_node: ROS node setup failed... \n" );

return -1;

}

arnl.robot->lock();

const std::map<int, ArLaser*> *lasers = arnl.robot->getLaserMap();

//---------------- This loop is not properly doing its job, working on effective fix ------------------------------

for(std::map<int, ArLaser*>::const_iterator i = lasers->begin(); i != lasers->end(); ++i)

{

ArLaser *l = i->second;

int ln = i->first;

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Creating publisher for laser #%d named %s\n", ln, l->getName());

std::string tfname("laser");

if(lasers->size() > 1 || ln > 1) // no number if only one laser which is also laser 1

tfname += ln;

tfname += "_frame";

new LaserPublisher(l, n, true, tfname);

}

//----------------------------------------------------------------------------------------------------------------

arnl.robot->unlock();

node->spin();

delete node;

ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Quitting... \n" );

Aria::exit(0);

return 0;