void autopilot_version_cb(const ros::TimerEvent &event) {
bool ret = false;
try {
auto client = nh.serviceClient<mavros::CommandLong>("cmd/command");
mavros::CommandLong cmd{};
cmd.request.command = MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES;
cmd.request.confirmation = false;
cmd.request.param1 = 1.0;
ROS_DEBUG_NAMED("sys", "VER: Sending request.");
ret = client.call(cmd);
}
catch (ros::InvalidNameException &ex) {
ROS_ERROR_NAMED("sys", "VER: %s", ex.what());
}
ROS_ERROR_COND_NAMED(!ret, "sys", "VER: command plugin service call failed!");
if (version_retries > 0) {
version_retries--;
ROS_WARN_COND_NAMED(version_retries != RETRIES_COUNT - 1, "sys",
"VER: request timeout, retries left %d", version_retries);
}
else {
uas->update_capabilities(false);
autopilot_version_timer.stop();
ROS_WARN_NAMED("sys", "VER: your FCU don't support AUTOPILOT_VERSION, "
"switched to default capabilities");
}
}
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());
}