/*!
* Gets the list of home poses.
*
* @param mapdata: Pointer to the map class.
*
* @return list of home objects.
*
*/
void
Advanced::getAllRobotHomes(ArMap* ariamap)
{
myHomeList.clear();
std::list<ArMapObject *>::iterator objIt;
ArMapObject* obj;
int i=0;
for (objIt = ariamap->getMapObjects()->begin();
objIt != ariamap->getMapObjects()->end();
objIt++)
{
//
// Get the forbidden lines and fill the occupancy grid there.
//
obj = (*objIt);
if (strcasecmp(obj->getType(), "RobotHome") == 0)
{
myHomeList.push_back(obj);
ArPose pose = obj->getPose();
printf("RobotHome[%d] = %s : %5.2f %5.2f %5.2f\n", i++, obj->getName(),
pose.getX(), pose.getY(), pose.getTh());
}
if (strcasecmp(obj->getType(), "Home") == 0)
{
myHomeList.push_back(obj);
ArPose pose = obj->getPose();
printf("Home[%d] = %s : %5.2f %5.2f %5.2f\n", i++, obj->getName(),
pose.getX(), pose.getY(), pose.getTh());
}
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
if(nrhs != 1)
mexErrMsgIdAndTxt( "arnetc:arnetc_robot_update_handler:minrhs", "Incorrect number of input arguments. Must provide robot update handler object reference returned by arnetc_new_robot_update_handler.");
uint64_t *p = (uint64_t*)mxGetData(prhs[0]);
ArClientHandlerRobotUpdate *updateHandler = (ArClientHandlerRobotUpdate*) *p;
if(nlhs < 1 || nlhs > 3)
mexErrMsgIdAndTxt("arnetc:arnetc_robot_update_handler:minlhs", "Incorrect number of output arguments. Must assign to a vector [x y theta] or to two scalars (x, y) or to three scalars (x, y, theta).");
updateHandler->lock();
ArPose pose = updateHandler->getPose();
updateHandler->unlock();
if(nlhs == 1)
{
plhs[0] = mxCreateDoubleMatrix(1, 3, mxREAL);
double* p = mxGetPr(plhs[0]);
p[0] = pose.getX();
p[1] = pose.getY();
p[2] = pose.getTh();
return;
}
if(nlhs >= 2)
{
plhs[0] = mxCreateDoubleScalar(pose.getX());
plhs[1] = mxCreateDoubleScalar(pose.getY());
if(nlhs > 2)
plhs[2] = mxCreateDoubleScalar(pose.getTh());
}
}
/*!
* This is the called when path to goal reached.
*
* @param goal: Goal it was assigned.
*/
void
Advanced::goalDone(ArPose goal)
{
static int goalCount = 0;
printf("goalDone:Main: %5.2f %5.2f %5.2f\07\n",
goal.getX(), goal.getY(), goal.getTh());
if(roundRobinFlag){
if(myLocaTask->getInitializedFlag()){
if(myGoalList.size()>0){
ArMapObject* front = myGoalList.front();
ArPose top = front->getPose();
bool headingFlag = false;
if(strcasecmp(front->getType(), "GoalWithHeading") == 0)
headingFlag = true;
else
headingFlag = false;
printf("Moving to next goal:%s %5.2f %5.2f %5.2f: %d poses %d Done\n",
front->getName(),
top.getX(), top.getY(), top.getTh(),
myGoalList.size(), goalCount++);
myGoalList.pop_front();
myGoalList.push_back(front);
myPathPlanningTask->pathPlanToPose(top, headingFlag);
}
}
}else{
printf("Localize the robot first\n");
}
}
/**
@param pose1 transform this into pose2
@param pose2 transform pose1 into this
*/
AREXPORT void ArTransform::setTransform(ArPose pose1, ArPose pose2)
{
myTh = ArMath::subAngle(pose2.getTh(), pose1.getTh());
myCos = ArMath::cos(-myTh);
mySin = ArMath::sin(-myTh);
myX = pose2.getX() - (myCos * pose1.getX() + mySin * pose1.getY());
myY = pose2.getY() - (myCos * pose1.getY() - mySin * pose1.getX());
}
int ballDetection::moveToBall() {
cout << "rufe Movetoball auf"<<endl;
//cout << "KoordX: "<< KoordX<<endl;
//int headingCount = 0;
double obj = findObj();
// kleiner wählen damit auf ball in größerer Entfernung gefunden wird
while(obj >= 0.5) {
p3dxptr->comInt(ArCommands::VEL, 0);
//ballimBild = true;
//cout << "#Drehungen: "<< headingCount << endl;
ArPose currentPose = p3dxptr->getPose();
cout<<"Koorigiere Heading "<<currentPose.getTh()<<endl;
if(KoordX >= 0 && KoordX <= 399) {
//p3dx.comInt(ArCommands::HEAD, i);
cout<<"Setze Heading -10 "<<endl;
// headingCount++;
p3dxptr->comInt(ArCommands::HEAD, (currentPose.getTh()+5));
ArUtil::sleep(500);
}
if(KoordX > 399 && KoordX <= 799) {
//p3dx.comInt(ArCommands::HEAD, i);
cout<<"Setze Heading +10 "<<endl;
// headingCount++;
p3dxptr->comInt(ArCommands::HEAD, currentPose.getTh()-5);
ArUtil::sleep(500);
}
if(rotateToBall() == 1) {
cout << "ball in der Mitte" <<endl;
p3dxptr->comInt(ArCommands::VEL, 75);
reached();
cout << "Rückgabe Wert 1" << endl;
return 1;
}else {
p3dxptr->comInt(ArCommands::VEL, 75);
p3dxptr->comInt(ArCommands::RVEL, 0);
cout << "Rückgabe Wert 0" << endl;
return 0;
}
}
if(obj < 0.5) {
return 0;
}
}
/*-------------------------------------------------------------
getOdometryFull
-------------------------------------------------------------*/
void CActivMediaRobotBase::getOdometryFull(
poses::CPose2D &out_odom,
double &out_lin_vel,
double &out_ang_vel,
int64_t &out_left_encoder_ticks,
int64_t &out_right_encoder_ticks
)
{
#if MRPT_HAS_ARIA
ASSERTMSG_(THE_ROBOT!=NULL, "Robot is not connected")
THE_ROBOT->lock();
// Odometry:
ArPose pose = THE_ROBOT->getEncoderPose();
out_odom.x( pose.getX() * 0.001 );
out_odom.y( pose.getY() * 0.001 );
out_odom.phi( DEG2RAD( pose.getTh() ) );
// Velocities:
out_lin_vel = THE_ROBOT->getVel() * 0.001;
out_ang_vel = DEG2RAD( THE_ROBOT->getRotVel() );
// Encoders:
out_left_encoder_ticks = THE_ROBOT->getLeftEncoder();
out_right_encoder_ticks = THE_ROBOT->getRightEncoder();
THE_ROBOT->unlock();
#else
MRPT_UNUSED_PARAM(out_odom); MRPT_UNUSED_PARAM(out_lin_vel); MRPT_UNUSED_PARAM(out_ang_vel);
MRPT_UNUSED_PARAM(out_left_encoder_ticks); MRPT_UNUSED_PARAM(out_right_encoder_ticks);
THROW_EXCEPTION("MRPT has been compiled with 'MRPT_BUILD_ARIA'=OFF, so this class cannot be used.");
#endif
}
virtual ArActionDesired *fire (ArActionDesired currentDesired)
{
if(BeenCorrectedByStar || !usestar) // if we have updated odo from star or is star isn't being used
{
ArPose MyPose;
MyPose = myRobot->getPose();
Bottle PoseBottle;
PoseBottle.addDouble(MyPose.getX()/1000);
PoseBottle.addDouble(MyPose.getY()/1000);
PoseBottle.addDouble(MyPose.getTh());
Mycopyofmodule->SendBottleData("MAPout",PoseBottle);
printf("my x:%f y:%f rot:%f \n",MyPose.getX()/1000,MyPose.getY()/1000,MyPose.getTh());
}
return &myDesired;
}
/**
* @brief read the robot's position and publish them to topic DEFAULT_TOPIC_ODOM
*/
void AriaCore::publish_pose()
{
ArPose pos;
m_robot.lock();
{
pos = m_robot.getPose();
}
m_robot.unlock();
// set position
nav_msgs::Odometry msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = m_frameIDOdom;
msg.child_frame_id = m_frameIDRobot;
tf::poseTFToMsg( tf::Pose(tf::Quaternion(0, 0, pos.getTh()*D2R),
tf::Vector3(pos.getX()/1000, pos.getY()/1000, 0)), msg.pose.pose);
// set vel
m_robot.lock();
{
msg.twist.twist.linear.x = m_robot.getVel()/1000.0;
msg.twist.twist.angular.z= m_robot.getRotVel()*D2R;
}
m_robot.unlock();
m_pubPos.publish(msg);
}
/*!
* Callback function for the l key. Localizes the robot at the home position.
* Warning: Robot must be physically at the first home pose.
*/
void
lkeyCB(void)
{
roundRobinFlag = false;
static int once = 0;
ArPose pose;
if(!once && advancedptr->myHomeList.size()>0){
once = 1;
ArPose top = advancedptr->myHomeList.front()->getPose();
printf("Localizing at %s %5.2f %5.2f %5.2f\n",
advancedptr->myHomeList.front()->getName(),
top.getX(), top.getY(), top.getTh());
//
// Set the pose to localize the robot about.
//
pose = top;
} else {
advancedptr->myRobot->lock();
pose = advancedptr->myRobot->getPose();
advancedptr->myRobot->unlock();
}
//
// Do the localization at the current pose.
//
if(advancedptr->localizeAtSetPose(pose)){
printf("Localized at current pose: Score: %5.2f out of 1.0\n",
advancedptr->getLocalizationScore());
}else {
printf("Failed to localize\n");
}
}
/*!
* Callback function for the r key. Gets the robot to pick the next
* goal point and plan to it. and keep going around the list.
*/
void
rkeyCB(void)
{
if(roundRobinFlag == false){
roundRobinFlag = true;
}else{
roundRobinFlag = false;
return;
}
if(advancedptr->myLocaTask->getInitializedFlag()){
if(advancedptr->myGoalList.size()>0){
ArMapObject* front = advancedptr->myGoalList.front();
ArPose top = front->getPose();
bool headingFlag = false;
if(strcasecmp(front->getType(), "GoalWithHeading") == 0)
headingFlag = true;
else
headingFlag = false;
printf("Path planing to goal %s %5.2f %5.2f %5.2f: %d poses\n",
front->getName(),
top.getX(), top.getY(), top.getTh(),
advancedptr->myGoalList.size());
advancedptr->myGoalList.pop_front();
advancedptr->myGoalList.push_back(front);
//
// Setup the pathplanning task to this destination.
//
advancedptr->pathPlanFromCurrentToDest(top, headingFlag);
}
}else{
printf("Localize the robot first\n");
}
}
/*-------------------------------------------------------------
getOdometryIncrement
-------------------------------------------------------------*/
void CActivMediaRobotBase::getOdometryIncrement(
poses::CPose2D &out_incr_odom,
double &out_lin_vel,
double &out_ang_vel,
int64_t &out_incr_left_encoder_ticks,
int64_t &out_incr_right_encoder_ticks
)
{
#if MRPT_HAS_ARIA
ASSERTMSG_(THE_ROBOT!=NULL, "Robot is not connected")
THE_ROBOT->lock();
static CPose2D last_pose;
static int64_t last_left_ticks=0, last_right_ticks=0;
CPose2D cur_pose;
int64_t cur_left_ticks, cur_right_ticks;
// Velocities:
out_lin_vel = THE_ROBOT->getVel() * 0.001;
out_ang_vel = DEG2RAD( THE_ROBOT->getRotVel() );
// Current odometry:
ArPose pose = THE_ROBOT->getEncoderPose();
cur_pose.x( pose.getX() * 0.001 );
cur_pose.y( pose.getY() * 0.001 );
cur_pose.phi( DEG2RAD( pose.getTh() ) );
// Current encoders:
cur_left_ticks = THE_ROBOT->getLeftEncoder();
cur_right_ticks = THE_ROBOT->getRightEncoder();
// Compute increment:
if (m_firstIncreOdometry)
{
// First time:
m_firstIncreOdometry = false;
out_incr_odom = CPose2D(0,0,0);
out_incr_left_encoder_ticks = 0;
out_incr_right_encoder_ticks = 0;
}
else
{
// Normal case:
out_incr_odom = cur_pose - last_pose;
out_incr_left_encoder_ticks = cur_left_ticks - last_left_ticks;
out_incr_right_encoder_ticks = cur_right_ticks - last_right_ticks;
}
// save for the next time:
last_pose = cur_pose;
last_left_ticks = cur_left_ticks;
last_right_ticks = cur_right_ticks;
THE_ROBOT->unlock();
#else
THROW_EXCEPTION("MRPT has been compiled with 'MRPT_BUILD_ARIA'=OFF, so this class cannot be used.");
#endif
}
/**
@param pose the coord system from which we transform to abs world coords
*/
AREXPORT void ArTransform::setTransform(ArPose pose)
{
myTh = pose.getTh();
myCos = ArMath::cos(-myTh);
mySin = ArMath::sin(-myTh);
myX = pose.getX();
myY = pose.getY();
}
void RosArnlNode::simple_goal_sub_cb(const geometry_msgs::PoseStampedConstPtr &msg)
{
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);
}
ArPose fn1(void)
{
static ArPose pose;
pose.setX(pose.getX() + 1);
pose.setY(pose.getY() + 1);
pose.setTh(pose.getTh() - 1);
return pose;
}
geometry_msgs::Pose arPoseToRosPose(const ArPose& arpose)
{
// 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;
}
/*!
* Interact with user on the terminal.
*/
void
interact()
{
ArMap* ariamap = advancedptr->myMap;
sleep(1);
advancedptr->getAllGoals(ariamap);
advancedptr->getAllRobotHomes(ariamap);
/// MPL
// lkeyCB();
advancedptr->myLocaTask->localizeRobotAtHomeNonBlocking();
//
// Interact with user using keyboard.
//
ArGlobalFunctor lCB(&lkeyCB);
ArGlobalFunctor pCB(&pkeyCB);
ArGlobalFunctor hCB(&hkeyCB);
ArGlobalFunctor rCB(&rkeyCB);
ArGlobalFunctor qCB(&quitCB);
ArGlobalFunctor escapeCB(&quitCB);
keyHandler.addKeyHandler('l', &lCB);
keyHandler.addKeyHandler('p', &pCB);
keyHandler.addKeyHandler('h', &hCB);
keyHandler.addKeyHandler('r', &rCB);
keyHandler.addKeyHandler('q', &qCB);
keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &escapeCB);
printf("Put robot at RobotHome and press 'l' to localize first.\n\
Press 'p' to move to the next goal\n\
Press 'h' to move to the first home\n\
Press 'r' to move to the goals in order\n\
Press 'q' to quit\n");
while (advancedptr->myLocaTask->getRunning() &&
advancedptr->myPathPlanningTask->getRunning()){
keyHandler.checkKeys();
ArUtil::sleep(250);
advancedptr->myRobot->lock();
ArPose rpose = advancedptr->myRobot->getPose();
double lvel = advancedptr->myRobot->getVel();
double avel = advancedptr->myRobot->getRotVel();
double volts = advancedptr->myRobot->getBatteryVoltage();
advancedptr->myRobot->unlock();
if(advancedptr->myLocaTask->getInitializedFlag()){
printf("\r%5.2f %5.2f %5.2f: %5.2f %5.2f: %4.1f\r",
rpose.getX(), rpose.getY(), rpose.getTh(), lvel, avel, volts);
fflush(stdout);
}
}
}
void ArSickLogger::internalPrintPos(ArPose poseTaken)
{
if (myFile == NULL)
return;
ArPose encoderPose = myRobot->getEncoderPose();
ArPose rawEncoderPose = myRobot->getRawEncoderPose();
ArTransform normalToRaw(rawEncoderPose, encoderPose);
ArPose rawPose;
rawPose = normalToRaw.doInvTransform(poseTaken);
fprintf(myFile, "#rawRobot: %.0f %.0f %.2f %.0f %.0f\n",
rawPose.getX(),
rawPose.getY(),
rawPose.getTh(),
myRobot->getVel(),
myRobot->getRotVel());
fprintf(myFile, "robot: %.0f %.0f %.2f %.0f %.0f\n",
poseTaken.getX(),
poseTaken.getY(),
poseTaken.getTh(),
myRobot->getVel(),
myRobot->getRotVel());
}
/*-------------------------------------------------------------
getOdometry
-------------------------------------------------------------*/
void CActivMediaRobotBase::getOdometry(poses::CPose2D &out_odom)
{
#if MRPT_HAS_ARIA
ASSERTMSG_(THE_ROBOT!=NULL, "Robot is not connected")
THE_ROBOT->lock();
ArPose pose = THE_ROBOT->getEncoderPose();
out_odom.x( pose.getX() * 0.001 );
out_odom.y( pose.getY() * 0.001 );
out_odom.phi( DEG2RAD( pose.getTh() ) );
THE_ROBOT->unlock();
#else
THROW_EXCEPTION("MRPT has been compiled with 'MRPT_BUILD_ARIA'=OFF, so this class cannot be used.");
#endif
}
/*!
* Callback function for the h key. Gets the robot to home pose.
*/
void
hkeyCB(void)
{
roundRobinFlag = false;
if(advancedptr->myLocaTask->getInitializedFlag()){
if(advancedptr->myHomeList.size()>0){
ArMapObject* front = advancedptr->myHomeList.front();
ArPose top = front->getPose();
printf("Homing to %s %5.2f %5.2f %5.2f\n",
front->getName(),
top.getX(), top.getY(), top.getTh());
bool headingFlag = true;
advancedptr->pathPlanFromCurrentToDest(top, headingFlag);
}
}else{
printf("Localize the robot first\n");
}
}
void RosArnlNode::publish()
{
// 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.
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);
if(action_executing)
{
move_base_msgs::MoveBaseFeedback feedback;
feedback.base_position.pose = pose_msg.pose.pose;
actionServer.publishFeedback(feedback);
}
// 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());
}
AREXPORT int ArInterpolation::getPose(ArTime timeStamp,
ArPose *position)
{
std::list<ArTime>::iterator tit;
std::list<ArPose>::iterator pit;
ArPose thisPose;
ArTime thisTime;
ArPose lastPose;
ArTime lastTime;
ArTime nowTime;
long total;
long toStamp;
double percentage;
ArPose retPose;
myDataMutex.lock();
// find the time we want
for (tit = myTimes.begin(), pit = myPoses.begin();
tit != myTimes.end() && pit != myPoses.end();
++tit, ++pit)
{
lastTime = thisTime;
lastPose = thisPose;
thisTime = (*tit);
thisPose = (*pit);
//printf("## %d %d %d b %d at %d after %d\n", timeStamp.getMSec(), thisTime.getMSec(), timeStamp.mSecSince(thisTime), timeStamp.isBefore(thisTime), timeStamp.isAt(thisTime), timeStamp.isAfter(thisTime));
//if (timeStamp.isBefore(thisTime) || timeStamp.isAt(thisTime))
if (!timeStamp.isAfter(thisTime))
{
//printf("Found one!\n");
break;
}
}
// if we're at the end then it was too long ago
if (tit == myTimes.end() || pit == myPoses.end())
{
//printf("Too old\n");
myDataMutex.unlock();
return -2;
}
// this is for forecasting (for the brave)
if ((tit == myTimes.begin() || pit == myPoses.begin()) &&
!timeStamp.isAt((*tit)))
{
//printf("Too new %d %d\n", tit == myTimes.begin(), pit == myPoses.begin());
thisTime = (*tit);
thisPose = (*pit);
tit++;
pit++;
if (tit == myTimes.end() || pit == myPoses.end())
{
//printf("Not enough data\n");
myDataMutex.unlock();
return -3;
}
lastTime = (*tit);
lastPose = (*pit);
nowTime.setToNow();
total = thisTime.mSecSince(lastTime);
if (total == 0)
total = 100;
toStamp = nowTime.mSecSince(thisTime);
percentage = (double)toStamp/(double)total;
//printf("Total time %d, to stamp %d, percentage %.2f\n", total, toStamp, percentage);
if (percentage > 50)
{
myDataMutex.unlock();
return -1;
}
retPose.setX(thisPose.getX() +
(thisPose.getX() - lastPose.getX()) * percentage);
retPose.setY(thisPose.getY() +
(thisPose.getY() - lastPose.getY()) * percentage);
retPose.setTh(ArMath::addAngle(thisPose.getTh(),
ArMath::subAngle(thisPose.getTh(),
lastPose.getTh())
* percentage));
*position = retPose;
myDataMutex.unlock();
return 0;
}
// this is the actual interpolation
//printf("Woo hoo!\n");
total = thisTime.mSecSince(lastTime);
toStamp = thisTime.mSecSince(timeStamp);
percentage = (double)toStamp/(double)total;
//printf("Total time %d, to stamp %d, percentage %.2f\n", total, toStamp, percentage);
retPose.setX(thisPose.getX() +
(lastPose.getX() - thisPose.getX()) * percentage);
retPose.setY(thisPose.getY() +
(lastPose.getY() - thisPose.getY()) * percentage);
retPose.setTh(ArMath::addAngle(thisPose.getTh(),
ArMath::subAngle(lastPose.getTh(),
thisPose.getTh())
* percentage));
/*
printf("original:");
thisPose.log();
printf("After:");
lastPose.log();
printf("ret:");
retPose.log();
*/
*position = retPose;
myDataMutex.unlock();
return 1;
}
/*!
* This is the called when path to goal fails
*
* @param goal: Goal it was assigned.
*/
void
Advanced::goalFailed(ArPose goal)
{
printf("goalFailed:Main: %5.2f %5.2f %5.2f\07\n",
goal.getX(), goal.getY(), goal.getTh());
}
void RosArnlNode::execute_action_cb(const move_base_msgs::MoveBaseGoalConstPtr &goal)
{
// 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;
}
void readPosition(ArRobot& robot){
ArPose pose = robot.getPose();
fseek(G_pose_fd, SEEK_SET, 0);
fprintf(G_pose_fd, "x=%0.6f;y=%0.6f;th=%0.6f;\n", pose.getX(), pose.getY(), pose.getTh());
}
void SamIter(void)
{
Bottle *goal = GoalIn.read(false);
//puts("running reader");
//while (GoalIn.getPendingReads() > 0)
// goal = GoalIn.read(false);
if(goal!=NULL) // check theres data
{
std::cout << "size of bottle is " << goal->size() << " data " << goal->toString().c_str() << std::endl;
if(goal->size()>0)
{
//iGoalPostion =GetGoalFromString(goal->get(1).asString().c_str());
iGoalPostion=atoi(goal->get(1).asString().c_str());
SetGoalPos(iGoalPostion);
bReached=false;
std::cout<< "got a goal, location " << iGoalPostion <<std::endl;
iStuckCount=0;
loopCnt=0;
robot->enableMotors();
robot->enableSonar();
}
}
//// send back a bottle with current voltage value
//Bottle& b2 = PwrOut.prepare(); // prepare the bottle/port
//b2.clear();
//b2.addDouble(robot->getRealBatteryVoltage()); // indicates robot voltage avg
//PwrOut.writeStrict();
Bottle *input = NULL;
//puts("running reader");
while (StarIn.getPendingReads() > 0)
input = StarIn.read(false);
if(input!=NULL) // check theres data
{
dStarId = input->get(0).asDouble();
//std::cout << "star id " << starID << std::endl;
dX = input->get(1).asDouble();
dY = input->get(2).asDouble();
dTh = input->get(3).asDouble();
if(firstStarReading==false)
{
CurrentPose.setX(dX);
CurrentPose.setY(dY);
CurrentPose.setTh(dTh);
OldPose = CurrentPose;
firstStarReading=true;
}
iStarCntr++;
if((OldPose.findDistanceTo(CurrentPose)<1.5 || iStarCntr>3) && dStarId!=0)
{
CurrentPose.setX(dX);
CurrentPose.setY(dY);
CurrentPose.setTh(dTh);
OldPose = CurrentPose;
iStarCntr=0;
}
//puts("got a msg");
//puts(input->toString());
//std::cout << " robot->getRealBatteryVoltageNow() " << " " << robot->getRealBatteryVoltageNow()<<std::endl;
}
if(bReached==false)
{
loopCnt++;
double ang = atan2(GoalPose.getY() - CurrentPose.getY(), GoalPose.getX() - CurrentPose.getX());
double dist = GoalPose.findDistanceTo(CurrentPose);
//check if the robot is stuck while navigating
if(dist>0)
{
//first distance reading
if(iStuckCount==0)
OldDist=dist;
DiffDist=OldDist-dist;
if(loopCnt%5==0 && abs(DiffDist)<0.1)
{
iStuckCount++;
std::cout << "diff dist, dist, stuck count " << DiffDist << " " << dist << " " << iStuckCount <<std::endl;
}
else
OldDist=dist;
//recover motion
if(iStuckCount>6)
{
robot->lock();
robot->setVel(-100);//move back
yarp::os::Time::delay(5);
robot->unlock();
//robot->setVel(0);//move back
iStuckCount=0;
}
}
//calculate_distance(dX, dY, GoalPose.getX(), GoalPose.getY());//ar.findDistanceTo(ArPose(-0.75,3.0,4.5));
ang-=dTh;
while(ang>3.14159265){ang-=6.28318531;} // normalise PI ie if over 180, then minus 360
while(ang<-3.14159265){ang+=6.28318531;}
ang = Rad2Deg(ang);
//std::cout << "required angle, dist " << ang << " " << dist <<std::endl;
//if the robot has to turn more then turn threshold for obst avoidance is less and speed is 0
if(abs(ang)>55)
{
myTurnThreshold = 250;
myStopDistance = 200;
speed=0;
}
else
{
speed=220;
myTurnThreshold = 550;
myStopDistance = 450;
}
bool obst= ObstacleAvoidance();
if(dist>=0.7 && obst==false)//&& abs(ang)>10
{
robot->lock();
std::cout <<"navigating to goal " << iGoalPostion<<std::endl;
/* if(abs(ang)<45)
speed=200;
else
speed=0;*/
robot->setDeltaHeading(ang/2.0);
robot->setVel(speed);
robot->unlock();
}
else if(dist<0.7)
{
robot->lock();
double rotFinal = GoalPose.getTh();
rotFinal-=dTh;
std::cout <<"goal rot " << rotFinal <<std::endl;
while(rotFinal>3.14159265){rotFinal-=6.28318531;} // normalise PI ie if over 180, then minus 360
while(rotFinal<-3.14159265){rotFinal+=6.28318531;}
rotFinal = Rad2Deg(rotFinal);
robot->setDeltaHeading(rotFinal);
robot->setVel(0);
std::cout <<"final rot " << rotFinal <<std::endl;
//robot->setDeltaHeading(0);
ang=0;
speed=0;
puts("reached ");
bReached=true;
robot->unlock();
// send back a bottle with goal position reached
Bottle& b2 = GoalIn.prepare(); // prepare the bottle/port
b2.clear();
b2.addInt(iGoalPostion); // indicates robot reached goal location
GoalIn.writeStrict();
//wait for some time for rotation to complete
yarp::os::Time::delay(5);
robot->disableMotors();
robot->disableSonar();
}
/*myfile.open ("motion.txt");
if (myfile.is_open())
{
myfile << ang << "|" << speed << "\n";
myfile.close();
}*/
}
//else{puts("didn't get a msg");}
}
void RosArnlNode::initialpose_sub_cb(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
{
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);
}
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);
}
}
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);
}
}