// copied from channelcontroller
double CalibrateAction::getDistanceAtPose(const tf::Pose & pose, bool* in_bounds) const
{
// determine current dist
int pose_x, pose_y;
costmap_.worldToMapNoBounds(pose.getOrigin().x(), pose.getOrigin().y(),
pose_x, pose_y);
//ROS_INFO("pose_x: %i, pose_y: %i", pose_x, pose_y);
if(pose_x < 0 || pose_y < 0 ||
pose_x >= (int)voronoi_.getSizeX() || pose_y >= (int)voronoi_.getSizeY()) {
if(in_bounds == NULL) {
// only warn if in_bounds isn't checked externally
ROS_WARN_THROTTLE(1.0, "%s: Pose out of voronoi bounds (%.2f, %.2f) = (%d, %d)", __func__,
pose.getOrigin().x(), pose.getOrigin().y(), pose_x, pose_y);
} else {
*in_bounds = false;
}
return 0.0;
}
if(in_bounds) {
*in_bounds = true;
}
float dist = voronoi_.getDistance(pose_x, pose_y);
dist *= costmap_.getResolution();
return dist;
}
void LoopClosure::visualizeEdge(const tf::Pose& pose1, const tf::Pose& pose2, visualization_msgs::MarkerArray& markers)
{
visualization_msgs::Marker marker;
marker.header.frame_id = "map";
marker.header.stamp = ros::Time::now();
marker.action = visualization_msgs::Marker::ADD;
marker.ns = "loop_closure";
marker.id = marker_id_++;
marker.type = visualization_msgs::Marker::LINE_STRIP;
geometry_msgs::Point p;
p.x = pose1.getOrigin().x();
p.y = pose1.getOrigin().y();
marker.points.push_back(p);
p.x = pose2.getOrigin().x();
p.y = pose2.getOrigin().y();
marker.points.push_back(p);
marker.scale.x = 0.25;
marker.scale.y = 0.25;
marker.scale.z = 0.25;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
//marker.lifetime = ros::Duration(5);
markers.markers.push_back(marker);
//marker_publisher_.publish(marker);
}
int CRvizMarker::Send(tf::Pose p, std::string frame) {
visualization_msgs::Marker marker;
// Set the frame ID and timestamp. See the TF tutorials for information on these.
marker.header.frame_id = frame;
marker.header.stamp = ros::Time::now();
// Set the namespace and id for this marker. This serves to create a unique ID
// Any marker sent with the same namespace and id will overwrite the old one
marker.ns = "nist_fanuc";
marker.id = _id++;
// Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
marker.type = shape;
// Set the marker action. Options are ADD and DELETE
marker.action = visualization_msgs::Marker::ADD;
// Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header
#if 0
marker.pose.position.x = 0;
marker.pose.position.y = 0;
marker.pose.position.z = 0;
marker.pose.orientation.x = 0.0;
marker.pose.orientation.y = 0.0;
marker.pose.orientation.z = 0.0;
marker.pose.orientation.w = 1.0;
#endif
marker.pose.position.x = p.getOrigin().x();
marker.pose.position.y = p.getOrigin().y();
marker.pose.position.z = p.getOrigin().z();
marker.pose.orientation.x = p.getRotation().x();
marker.pose.orientation.y = p.getRotation().y();
marker.pose.orientation.z = p.getRotation().z();
marker.pose.orientation.w = p.getRotation().w();
// Set the scale of the marker -- 1x1x1 here means 1m on a side!!!
marker.scale.x = scalex;
marker.scale.y = scaley;
marker.scale.z = scalez;
// Set the color -- be sure to set alpha to something non-zero!
marker.color.r = r;
marker.color.g = g;
marker.color.b = b;
marker.color.a = a;
marker.lifetime = ros::Duration(0.0);
// Publish the marker
marker_pub.publish(marker);
ros::spinOnce();
ros::spinOnce();
return 0;
}
MathLib::Matrix4 toMatrix4(const tf::Pose& pose) {
MathLib::Matrix4 mat;
mat.Identity();
tf::Matrix3x3 mat33(pose.getRotation());
mat.SetTranslation(MathLib::Vector3(pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z()));
mat.SetOrientation(MathLib::Matrix3(mat33[0][0], mat33[0][1], mat33[0][2],
mat33[1][0], mat33[1][1], mat33[1][2],
mat33[2][0], mat33[2][1], mat33[2][2]));
return mat;
}
geometry_msgs::Pose tfTransformToGeometryPose(const tf::Pose& goal_pose)
{
geometry_msgs::Pose target_pose1;
target_pose1.orientation.x = goal_pose.getRotation().getX();
target_pose1.orientation.y = goal_pose.getRotation().getY();
target_pose1.orientation.z = goal_pose.getRotation().getZ();
target_pose1.orientation.w = goal_pose.getRotation().getW();
target_pose1.position.x = goal_pose.getOrigin().getX(); // + std::sin(angle)*radius;
target_pose1.position.y = goal_pose.getOrigin().getY(); // + std::cos(angle)*radius;
target_pose1.position.z = goal_pose.getOrigin().getZ();
return target_pose1;
}
void toMsgPose(const tf::Pose& tf_pose, geometry_msgs::Pose& msg_pose)
{
msg_pose.position.x = tf_pose.getOrigin().getX();
msg_pose.position.y = tf_pose.getOrigin().getY();
msg_pose.position.z = tf_pose.getOrigin().getZ();
tf::Quaternion orientation = tf_pose.getRotation();
msg_pose.orientation.w = orientation.getW();
msg_pose.orientation.x = orientation.getX();
msg_pose.orientation.y = orientation.getY();
msg_pose.orientation.z = orientation.getZ();
}
geometry_msgs::Pose tfPoseToGeometryPose(const tf::Pose tPose)
{
geometry_msgs::Pose gPose;
gPose.position.x = tPose.getOrigin().x();
gPose.position.y = tPose.getOrigin().y();
gPose.position.z = tPose.getOrigin().z();
gPose.orientation.x = tPose.getRotation().x();
gPose.orientation.y = tPose.getRotation().y();
gPose.orientation.z = tPose.getRotation().z();
gPose.orientation.w = tPose.getRotation().w();
return gPose;
}
void MotionModel::setMotion(const tf::Pose& pnew, const tf::Pose& pold)
{
tf::Pose delta_pose;
tf::Transform odom_to_base(pold.inverse().getRotation());
delta_pose.setOrigin(odom_to_base * (pnew.getOrigin() - pold.getOrigin()));
delta_pose.setRotation(pnew.getRotation() * pold.getRotation().inverse());
delta_x = delta_pose.getOrigin().x();
delta_y = delta_pose.getOrigin().y();
delta_yaw = atan2(sin(tf::getYaw(delta_pose.getRotation())), cos(tf::getYaw(delta_pose.getRotation())));
dxy=sqrt(delta_x*delta_x+delta_y*delta_y);
}
// The famous sendPose!
void sendPose(const tf::Pose& pose_) {
geometry_msgs::PoseStamped msg;
msg.pose.position.x = pose_.getOrigin().x();
msg.pose.position.y = pose_.getOrigin().y();
msg.pose.position.z = pose_.getOrigin().z();
msg.pose.orientation.x = pose_.getRotation().x();
msg.pose.orientation.y = pose_.getRotation().y();
msg.pose.orientation.z = pose_.getRotation().z();
msg.pose.orientation.w = pose_.getRotation().w();
pub_.publish(msg);
}
static RVector<double> PoseValues(tf::Pose & pose) {
RVector<double> values;
values.push_back( pose.getOrigin().x());
values.push_back( pose.getOrigin().y());
values.push_back( pose.getOrigin().z());
double roll, pitch, yaw;
tf::Quaternion q = pose.getRotation();
tf::Matrix3x3(q).getRPY(roll, pitch, yaw );
values.push_back( roll );
values.push_back( pitch );
values.push_back( yaw );
return values;
}
void poseTFToKDL(const tf::Pose& t, KDL::Frame& k)
{
for (unsigned int i = 0; i < 3; ++i)
k.p[i] = t.getOrigin()[i];
for (unsigned int i = 0; i < 9; ++i)
k.M.data[i] = t.getBasis()[i/3][i%3];
}
tf::Pose MotionModel::updateAction(tf::Pose& p)
{
double delta_rot1;
if (dxy < 0.01)
delta_rot1 = 0.0;
else
delta_rot1 = angle_diff(atan2(delta_y, delta_x), delta_yaw);
double delta_trans = dxy;
double delta_rot2 = angle_diff(delta_yaw, delta_rot1);
double delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)), fabs(angle_diff(delta_rot1, M_PI)));
double delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)), fabs(angle_diff(delta_rot2, M_PI)));
double delta_rot1_hat = angle_diff(delta_rot1, sampleGaussian(alpha1 * delta_rot1_noise*delta_rot1_noise +
alpha2 * delta_trans*delta_trans));
double delta_trans_hat = delta_trans - sampleGaussian(alpha3 * delta_trans*delta_trans +
alpha4 * delta_rot1_noise*delta_rot1_noise +
alpha4 * delta_rot2_noise*delta_rot2_noise);
double delta_rot2_hat = angle_diff(delta_rot2, sampleGaussian(alpha1 * delta_rot2_noise*delta_rot2_noise +
alpha2 * delta_trans*delta_trans));
delta_x = delta_trans_hat * cos(delta_rot1_hat);
delta_y = delta_trans_hat * sin(delta_rot1_hat);
delta_yaw = delta_rot1_hat + delta_rot2_hat;
tf::Pose noisy_pose(tf::createQuaternionFromRPY(0,0,delta_yaw),tf::Vector3(delta_x,delta_y,0));
tf::Transform base_to_global_ = tf::Transform(p.getRotation());
noisy_pose.setOrigin(base_to_global_ * noisy_pose.getOrigin());
p.setOrigin(p.getOrigin() + noisy_pose.getOrigin());
p.setRotation(p.getRotation() * noisy_pose.getRotation());
}
void StepCostKey::mirrorPoseOnXPlane(tf::Pose &mirror, const tf::Pose &orig) const
{
mirror.setBasis(orig.getBasis());
tf::Matrix3x3& basis = mirror.getBasis();
basis[0][1] = -basis[0][1];
basis[1][0] = -basis[1][0];
basis[2][1] = -basis[2][1];
mirror.setOrigin(orig.getOrigin());
tf::Vector3& origin = mirror.getOrigin();
origin[1] = -origin[1];
// double r, p, y;
// tf::Matrix3x3(orig.getRotation()).getRPY(r, p, y);
// mirror.setRotation(tf::createQuaternionFromRPY(-r, p, -y));
// tf::Vector3 v = orig.getOrigin();
// v.setY(-v.getY());
// mirror.setOrigin(v);
// tfScalar m[16];
// ROS_INFO("------------------");
// mirror.getOpenGLMatrix(m);
// ROS_INFO("%f %f %f %f", m[0], m[4], m[8], m[12]);
// ROS_INFO("%f %f %f %f", m[1], m[5], m[9], m[13]);
// ROS_INFO("%f %f %f %f", m[2], m[6], m[10], m[14]);
// ROS_INFO("%f %f %f %f", m[3], m[7], m[11], m[15]);
// ROS_INFO("-");
// orig.getOpenGLMatrix(m);
// ROS_INFO("%f %f %f %f", m[0], m[4], m[8], m[12]);
// ROS_INFO("%f %f %f %f", m[1], m[5], m[9], m[13]);
// ROS_INFO("%f %f %f %f", m[2], m[6], m[10], m[14]);
// ROS_INFO("%f %f %f %f", m[3], m[7], m[11], m[15]);
// ROS_INFO("-");
// ROS_INFO("%f %f %f %f", m[0], -m[4], m[8], m[12]);
// ROS_INFO("%f %f %f %f", -m[1], m[5], m[9], -m[13]);
// ROS_INFO("%f %f %f %f", m[2], -m[6], m[10], m[14]);
// ROS_INFO("%f %f %f %f", m[3], m[7], m[11], m[15]);
}
geometry_msgs::Twist PoseFollower::diff2D(const tf::Pose& pose1, const tf::Pose& pose2)
{
geometry_msgs::Twist res;
tf::Pose diff = pose2.inverse() * pose1;
res.linear.x = diff.getOrigin().x();
res.linear.y = diff.getOrigin().y();
res.angular.z = tf::getYaw(diff.getRotation());
if(holonomic_ || (fabs(res.linear.x) <= tolerance_trans_ && fabs(res.linear.y) <= tolerance_trans_))
return res;
//in the case that we're not rotating to our goal position and we have a non-holonomic robot
//we'll need to command a rotational velocity that will help us reach our desired heading
//we want to compute a goal based on the heading difference between our pose and the target
double yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(),
pose2.getOrigin().x(), pose2.getOrigin().y(), tf::getYaw(pose2.getRotation()));
//---------------Modified----------------------
//we'll also check if we can move more effectively backwards
//double neg_yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(),
// pose2.getOrigin().x(), pose2.getOrigin().y(), M_PI + tf::getYaw(pose2.getRotation()));
//check if its faster to just back up
// if(fabs(neg_yaw_diff) < fabs(yaw_diff)){
// ROS_DEBUG("Negative is better: %.2f", neg_yaw_diff);
// yaw_diff = neg_yaw_diff;
// }
//compute the desired quaterion
tf::Quaternion rot_diff = tf::createQuaternionFromYaw(yaw_diff);
tf::Quaternion rot = pose2.getRotation() * rot_diff;
tf::Pose new_pose = pose1;
new_pose.setRotation(rot);
diff = pose2.inverse() * new_pose;
res.linear.x = diff.getOrigin().x();
res.linear.y = diff.getOrigin().y();
res.angular.z = tf::getYaw(diff.getRotation());
return res;
}
// Go to this pose
bool go_home(tf::Pose& pose_) {
tf::Vector3 start_p(pose_.getOrigin());
tf::Quaternion start_o(pose_.getRotation());
msg_pose.pose.position.x = start_p.x();
msg_pose.pose.position.y = start_p.y();
msg_pose.pose.position.z = start_p.z();
msg_pose.pose.orientation.w = start_o.w();
msg_pose.pose.orientation.x = start_o.x();
msg_pose.pose.orientation.y = start_o.y();
msg_pose.pose.orientation.z = start_o.z();
pub_.publish(msg_pose);
sendNormalForce(0);
ros::Rate thread_rate(2);
ROS_INFO("Homing...");
while(ros::ok()) {
double oerr =(ee_pose.getRotation() - start_o).length() ;
double perr = (ee_pose.getOrigin() - start_p).length();
feedback_.progress = 0.5*(perr+oerr);
as_.publishFeedback(feedback_);
ROS_INFO_STREAM("Error: "<<perr<<", "<<oerr);
if(perr< 0.02 && oerr < 0.02) {
break;
}
if (as_.isPreemptRequested() || !ros::ok() )
{
sendNormalForce(0);
sendPose(ee_pose);
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
return false;
}
thread_rate.sleep();
}
return ros::ok();
}
// Roll with "force" and horizontal "speed" until the length "range"
bool rolling(double force, double speed, double range) {
ROS_INFO_STREAM("Rolling with force "<<force<<", speed "<<speed<<", range "<<range);
force = fabs(force);
sendNormalForce(-force);
msg_pose.pose.position.x = ee_pose.getOrigin().x();
msg_pose.pose.position.y = ee_pose.getOrigin().y();
msg_pose.pose.position.z = ee_pose.getOrigin().z();
tf::Quaternion q = ee_pose.getRotation();
msg_pose.pose.orientation.x = q.x();
msg_pose.pose.orientation.y = q.y();
msg_pose.pose.orientation.z = q.z();
msg_pose.pose.orientation.w = q.w();
double center = ee_pose.getOrigin().y();
double rate = 200;
ros::Rate thread_rate(rate);
int count=0;
while(ros::ok()) {
msg_pose.pose.position.y = msg_pose.pose.position.y + speed/rate;
feedback_.progress = msg_pose.pose.position.y;
as_.publishFeedback(feedback_);
if( fabs(msg_pose.pose.position.y - center) > range) {
ROS_INFO("Change direction");
speed *= -1;
if(++count > 5) {
break;
}
}
pub_.publish(msg_pose);
thread_rate.sleep();
}
msg_pose.pose.position.z = ee_pose.getOrigin().z() + 0.15;
pub_.publish(msg_pose);
sendNormalForce(0);
return true;
}
// Convert from tf::Pose to CameraPose (position and orientation)
inline void TFPose2CameraPose(const tf::Pose& pose, CameraPose& cameraPose)
{
// convert to position opencv vector
tf::Vector3 position_tf = pose.getOrigin();
cv::Point3d position = cv::Point3d(position_tf.getX(), position_tf.getY(), position_tf.getZ());
// Convert to orientation opencv quaternion
tf::Quaternion orientation_tf = pose.getRotation();
cv::Vec4d orientation(orientation_tf.getW(), orientation_tf.getX(), orientation_tf.getY(), orientation_tf.getZ());
cameraPose = CameraPose(position, orientation);
}
// Callback for the desired cartesian pose
void cartCallback(const geometry_msgs::PoseStampedConstPtr& msg) {
ROS_INFO_STREAM("Received Position Command");
const geometry_msgs::PoseStamped* data = msg.get();
des_ee_pose.setOrigin(tf::Vector3(data->pose.position.x,data->pose.position.y,data->pose.position.z));
des_ee_pose.setRotation(tf::Quaternion(data->pose.orientation.x,data->pose.orientation.y,data->pose.orientation.z,data->pose.orientation.w));
ROS_INFO_STREAM_THROTTLE(1, "Received Position: "<<des_ee_pose.getOrigin().x()<<","<<des_ee_pose.getOrigin().y()<<","<<des_ee_pose.getOrigin().z());
if(bOrientCtrl) {
tf::Quaternion q = des_ee_pose.getRotation();
ROS_INFO_STREAM_THROTTLE(1, "Received Orientation: "<<q.x()<<","<<q.y()<<","<<q.z()<<","<<q.w());
}
}
// Go down until hit the table. For safety min_height is specified. If no table found until this height, returns false.
// vertical_speed with which to move downwards
// thr_force - normal force threshold at which table is assumed to be detected
bool find_table_for_rolling(double min_height, double vertical_speed, double thr_force) {
double rate = 200;
thr_force = fabs(thr_force);
ros::Rate thread_rate(rate);
double startz = ee_pose.getOrigin().z();
msg_pose.pose.position.x = ee_pose.getOrigin().x();
msg_pose.pose.position.y = ee_pose.getOrigin().y();
msg_pose.pose.position.z = startz;
msg_pose.pose.orientation.x = ee_pose.getRotation().x();
msg_pose.pose.orientation.y = ee_pose.getRotation().y();
msg_pose.pose.orientation.z = ee_pose.getRotation().z();
msg_pose.pose.orientation.w = ee_pose.getRotation().w();
// Publish attractors if running in simulation or with fixed values
if ((action_mode == ACTION_LASA_FIXED) || (action_mode == ACTION_BOXY_FIXED)) {
static tf::TransformBroadcaster br;
tf::Transform table;
table.setOrigin(tf::Vector3 (ee_pose.getOrigin().x(),ee_pose.getOrigin().y(),ee_pose.getOrigin().z() - min_height));
table.setRotation(tf::Quaternion (ee_pose.getRotation().x(),ee_pose.getRotation().y(),ee_pose.getRotation().z(),ee_pose.getRotation().w()));
br.sendTransform(tf::StampedTransform(table, ros::Time::now(), world_frame, "/attractor"));
}
ROS_INFO_STREAM("Finding table up to max dist. "<<min_height<<" with vertical speed "<<vertical_speed<<" and threshold force "<<thr_force<<"N.");
while(ros::ok()) {
msg_pose.pose.position.z = msg_pose.pose.position.z - vertical_speed/rate;
pub_.publish(msg_pose);
// Go down until force reaches the threshold
if(fabs(ee_ft[2]) > thr_force) {
break;
}
if(fabs(ee_pose.getOrigin().z()-startz) > min_height) {
ROS_INFO("Max distance reached");
return false;
}
thread_rate.sleep();
feedback_.progress = ee_ft[2];
as_.publishFeedback(feedback_);
}
if(!ros::ok()) {
return false;
}
tf::Vector3 table(ee_pose.getOrigin());
ROS_INFO_STREAM("Table found at height "<<table[2]);
msg_pose.pose.position.z = table[2];
pub_.publish(msg_pose);
sendAndWaitForNormalForce(0);
return true;
}
void sendPose(tf::Pose& pose) {
msg_pose.header.stamp = ros::Time::now();
tf::Vector3 tmp = pose.getOrigin();
msg_pose.pose.position.x = tmp.x();
msg_pose.pose.position.y = tmp.y();
msg_pose.pose.position.z = tmp.z();
tf::Quaternion quat = pose.getRotation();
msg_pose.pose.orientation.w = quat.w();
msg_pose.pose.orientation.x = quat.x();
msg_pose.pose.orientation.y = quat.y();
msg_pose.pose.orientation.z = quat.z();
pub_pose.publish(msg_pose);
}
gs::LocalizationDistribution::Ptr OdomLocalizer::update (const Graph&,
gs::LocalizationDistribution::ConstPtr localization,
const tf::Pose& rel_pose, const ScanPtr& scan)
{
gs::LocalizationDistribution::Ptr dist(new gs::LocalizationDistribution());
dist->stamp = scan->header.stamp;
dist->samples.resize(1);
ROS_ASSERT(localization->samples.size()==1);
dist->samples[0].node = localization->samples[0].node;
dist->samples[0].offset = util::absolutePose(localization->samples[0].offset, rel_pose);
ROS_DEBUG_STREAM_COND_NAMED (fabs(rel_pose.getOrigin().x()) > 1e-3,
"localization", "In odom localization update with relative pose " <<
util::toString(rel_pose) << "; old loc was " << localization->samples[0] <<
" and new is " << dist->samples[0]);
return dist;
}
double getNearestDoorAngle(const tf::Pose& robot_pose, const door_msgs::Door& door, double robot_dist, double touch_dist)
{
double angle = 0, step = 0;
if (door.rot_dir == door.ROT_DIR_CLOCKWISE)
step = -0.01;
else if (door.rot_dir == door.ROT_DIR_COUNTERCLOCKWISE)
step = 0.01;
else{
ROS_ERROR("Door rot dir is not specified");
return 0.0;
}
while ((robot_pose.getOrigin() - getGripperPose(door, angle, touch_dist).getOrigin()).length() < robot_dist &&
fabs(angle) < M_PI_2)
angle += step;
return angle;
}
tf::Pose MotionModel::drawFromMotion(tf::Pose& p)
{
double sxy=0.3*srr;
delta_x+=sampleGaussian(srr*fabs(delta_x)+str*fabs(delta_yaw)+sxy*fabs(delta_y));
delta_y+=sampleGaussian(srr*fabs(delta_y)+str*fabs(delta_yaw)+sxy*fabs(delta_x));
delta_yaw+=sampleGaussian(stt*fabs(delta_yaw)+srt*dxy);
delta_yaw=fmod(delta_yaw, 2*M_PI);
if (delta_yaw>M_PI)
delta_yaw-=2*M_PI;
tf::Pose noisy_pose(tf::createQuaternionFromRPY(0,0,delta_yaw),tf::Vector3(delta_x,delta_y,0));
tf::Transform base_to_global_ = tf::Transform(p.getRotation());
noisy_pose.setOrigin(base_to_global_ * noisy_pose.getOrigin());
p.setOrigin(p.getOrigin() + noisy_pose.getOrigin());
p.setRotation(p.getRotation() * noisy_pose.getRotation());
return p;
}
void LoopClosure::visualizeNode(const tf::Pose& pose, visualization_msgs::MarkerArray& markers)
{
visualization_msgs::Marker marker;
marker.header.frame_id = "map";
marker.header.stamp = ros::Time::now();
marker.action = visualization_msgs::Marker::ADD;
marker.ns = "loop_closure";
marker.id = marker_id_++;
marker.type = visualization_msgs::Marker::SPHERE;
marker.pose.position.x = pose.getOrigin().x();
marker.pose.position.y = pose.getOrigin().y();
marker.scale.x = 0.5;
marker.scale.y = 0.5;
marker.scale.z = 0.5;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 0.0;
marker.color.b = 0.0;
//marker.lifetime = ros::Duration(5);
markers.markers.push_back(marker);
//marker_publisher_.publish(marker);
}
bool
LoopClosure::checkLoopClosure(const tf::Pose& pose, std::vector<GraphNode*>& candidates)
{
bool ret = false;
if(curr_node_ == NULL || nodes_.size() < 2)
return ret;
// Compute shortest distances from current node to all nodes. Distances
// are stored in nodes themselves.
dijkstra(curr_node_->id_);
//we'll only compute the potential once
geometry_msgs::Pose temp_pose;
geometry_msgs::PoseStamped planner_start;
tf::poseTFToMsg(pose, temp_pose);
planner_start.header.stamp = ros::Time::now();
planner_start.header.frame_id = costmap_.getGlobalFrameID();
planner_start.pose = temp_pose;
planner_->computePotential(planner_start.pose.position);
//get a copy of the costmap that we can do raytracing on
costmap_2d::Costmap2D *visibility_map = costmap_.getCostmap();
for(std::vector<GraphNode*>::iterator it = nodes_.begin();
it != nodes_.end();
++it)
{
ROS_DEBUG("Checking %d (%.3f, %.3f)", (*it)->id_, (*it)->pose_.getOrigin().x(), (*it)->pose_.getOrigin().y());
ROS_DEBUG("Graph distance is %.3f (threshold is %.3f)", (*it)->dijkstra_d_, loop_dist_max_);
if((*it)->dijkstra_d_ > loop_dist_max_)
{
ROS_DEBUG("graph dist check satisfied");
//we need to convert from tf to message types for the planner
geometry_msgs::PoseStamped planner_end;
tf::poseTFToMsg((*it)->pose_, temp_pose);
planner_end.header.stamp = ros::Time::now();
planner_end.header.frame_id = costmap_.getGlobalFrameID();
planner_end.pose = temp_pose;
//now we'll make a plan from one point to the other
std::vector<geometry_msgs::PoseStamped> plan;
bool valid_plan = planner_->getPlanFromPotential(planner_end, plan) && !plan.empty();
// Graph distance check satisfied; compute map distance
if(valid_plan){
double path_length = 0.0;
//compute the path length
if(plan.size() > 1){
//double dist = distance(plan[0], plan[1]);
//path_length = dist * (plan.size() - 1);
for(unsigned int j = 0; j < plan.size() - 1; ++j){
double dist = distance(plan[j], plan[j + 1]);
path_length += dist;
}
ROS_DEBUG("path_length: %.3f (threshold is %.3f)", path_length, loop_dist_min_);
if(path_length < loop_dist_min_)
{
//also check that there is visibility from one node to the other
bool is_visible = true;
VisibilityChecker checker(visibility_map->getCharMap(), is_visible);
//get map coordinates
bool in_bounds = true;
unsigned int x0, y0;
if(!visibility_map->worldToMap(pose.getOrigin().x(), pose.getOrigin().y(), x0, y0)){
ROS_WARN("Attempting to check visibility from a point off the map... this should never happen");
in_bounds = false;
}
unsigned int x1, y1;
if(!visibility_map->worldToMap((*it)->pose_.getOrigin().x(), (*it)->pose_.getOrigin().y(), x1, y1)){
ROS_WARN("Attempting to check visibility to a point off the map... this should never happen");
in_bounds = false;
}
if(in_bounds){
//raytrace a line with our visibility checker
raytraceLine(checker, x0, y0, x1, y1, visibility_map->getSizeInCellsX());
//check if we have visibility to the node
if(is_visible){
//update the path distance on the node we're about to push back
(*it)->path_length_ = path_length;
// Both checks satisfied
candidates.push_back(*it);
ROS_INFO("added loop closure candidate %d (%.3f, %.3f)",
(*it)->id_, (*it)->pose_.getOrigin().x(), (*it)->pose_.getOrigin().y());
ret = true;
}
}
}
}
}
}
}
return ret;
}
void NaoqiJointStates::run()
{
ros::Rate r(m_rate);
ros::Time stamp1;
ros::Time stamp2;
ros::Time stamp;
std::vector<float> odomData;
float odomX, odomY, odomZ, odomWX, odomWY, odomWZ;
std::vector<float> memData;
std::vector<float> positionData;
ROS_INFO("Staring main loop. ros::ok() is %d nh.ok() is %d",ros::ok(),m_nh.ok());
while(ros::ok() )
{
r.sleep();
ros::spinOnce();
stamp1 = ros::Time::now();
try
{
memData = m_memoryProxy->getListData(m_dataNamesList);
// {SPACE_TORSO = 0, SPACE_WORLD = 1, SPACE_NAO = 2}. (second argument)
odomData = m_motionProxy->getPosition("Torso", 1, true);
positionData = m_motionProxy->getAngles("Body", true);
}
catch (const AL::ALError & e)
{
ROS_ERROR( "Error accessing ALMemory, exiting...");
ROS_ERROR( "%s", e.what() );
//ros.signal_shutdown("No NaoQI available anymore");
}
stamp2 = ros::Time::now();
//ROS_DEBUG("dt is %f",(stamp2-stamp1).toSec()); % dt is typically around 1/1000 sec
// TODO: Something smarter than this..
stamp = stamp1 + ros::Duration((stamp2-stamp1).toSec()/2.0);
/******************************************************************
* IMU
*****************************************************************/
if (memData.size() != m_dataNamesList.getSize())
{
ROS_ERROR("memData length %zu does not match expected length %u",memData.size(),m_dataNamesList.getSize() );
continue;
}
// IMU data:
m_torsoIMU.header.stamp = stamp;
float angleX = memData[1];
float angleY = memData[2];
float angleZ = memData[3];
float gyroX = memData[4];
float gyroY = memData[5];
float gyroZ = memData[6];
float accX = memData[7];
float accY = memData[8];
float accZ = memData[9];
m_torsoIMU.orientation = tf::createQuaternionMsgFromRollPitchYaw(
angleX,
angleY,
angleZ); // yaw currently always 0
m_torsoIMU.angular_velocity.x = gyroX;
m_torsoIMU.angular_velocity.y = gyroY;
m_torsoIMU.angular_velocity.z = gyroZ; // currently always 0
m_torsoIMU.linear_acceleration.x = accX;
m_torsoIMU.linear_acceleration.y = accY;
m_torsoIMU.linear_acceleration.z = accZ;
// covariances unknown
// cf http://www.ros.org/doc/api/sensor_msgs/html/msg/Imu.html
m_torsoIMU.orientation_covariance[0] = -1;
m_torsoIMU.angular_velocity_covariance[0] = -1;
m_torsoIMU.linear_acceleration_covariance[0] = -1;
m_torsoIMUPub.publish(m_torsoIMU);
/******************************************************************
* Joint state
*****************************************************************/
m_jointState.header.stamp = stamp;
m_jointState.header.frame_id = m_baseFrameId;
m_jointState.position.resize(positionData.size());
for(unsigned i = 0; i<positionData.size(); ++i)
{
m_jointState.position[i] = positionData[i];
}
// simulated model misses some joints, we need to fill:
if (m_jointState.position.size() == 22)
{
m_jointState.position.insert(m_jointState.position.begin()+6,0.0);
m_jointState.position.insert(m_jointState.position.begin()+7,0.0);
m_jointState.position.push_back(0.0);
m_jointState.position.push_back(0.0);
}
m_jointStatePub.publish(m_jointState);
/******************************************************************
* Odometry
*****************************************************************/
if (!m_paused) {
// apply offset transformation:
tf::Pose transformedPose;
if (odomData.size()!=6)
{
ROS_ERROR( "Error getting odom data. length is %zu",odomData.size() );
continue;
}
double dt = (stamp.toSec() - m_lastOdomTime);
odomX = odomData[0];
odomY = odomData[1];
odomZ = odomData[2];
odomWX = odomData[3];
odomWY = odomData[4];
odomWZ = odomData[5];
tf::Quaternion q;
// roll and pitch from IMU, yaw from odometry:
if (m_useIMUAngles)
q.setRPY(angleX, angleY, odomWZ);
else
q.setRPY(odomWX, odomWY, odomWZ);
m_odomPose.setOrigin(tf::Vector3(odomX, odomY, odomZ));
m_odomPose.setRotation(q);
if(m_mustUpdateOffset) {
if(!m_isInitialized) {
if(m_initializeFromIMU) {
// Initialization from IMU: Take x, y, z, yaw from odometry, roll and pitch from IMU
m_targetPose.setOrigin(m_odomPose.getOrigin());
m_targetPose.setRotation(tf::createQuaternionFromRPY(angleX, angleY, odomWZ));
} else if(m_initializeFromOdometry) {
m_targetPose.setOrigin(m_odomPose.getOrigin());
m_targetPose.setRotation(tf::createQuaternionFromRPY(odomWX, odomWY, odomWZ));
}
m_isInitialized = true;
} else {
// Overwrite target pitch and roll angles with IMU data
const double target_yaw = tf::getYaw(m_targetPose.getRotation());
if(m_initializeFromIMU) {
m_targetPose.setRotation(tf::createQuaternionFromRPY(angleX, angleY, target_yaw));
} else if(m_initializeFromOdometry){
m_targetPose.setRotation(tf::createQuaternionFromRPY(odomWX, odomWY, target_yaw));
}
}
m_odomOffset = m_targetPose * m_odomPose.inverse();
transformedPose = m_targetPose;
m_mustUpdateOffset = false;
} else {
transformedPose = m_odomOffset * m_odomPose;
}
//
// publish the transform over tf first
//
m_odomTransformMsg.header.stamp = stamp;
tf::transformTFToMsg(transformedPose, m_odomTransformMsg.transform);
m_transformBroadcaster.sendTransform(m_odomTransformMsg);
//
// Fill the Odometry msg
//
m_odom.header.stamp = stamp;
//set the velocity first (old values still valid)
m_odom.twist.twist.linear.x = (odomX - m_odom.pose.pose.position.x) / dt;
m_odom.twist.twist.linear.y = (odomY - m_odom.pose.pose.position.y) / dt;
m_odom.twist.twist.linear.z = (odomZ - m_odom.pose.pose.position.z) / dt;
// TODO: calc angular velocity!
// m_odom.twist.twist.angular.z = vth; ??
//set the position from the above calculated transform
m_odom.pose.pose.orientation = m_odomTransformMsg.transform.rotation;
m_odom.pose.pose.position.x = m_odomTransformMsg.transform.translation.x;
m_odom.pose.pose.position.y = m_odomTransformMsg.transform.translation.y;
m_odom.pose.pose.position.z = m_odomTransformMsg.transform.translation.z;
m_odomPub.publish(m_odom);
m_lastOdomTime = stamp.toSec();
}
}
ROS_INFO("naoqi_sensors stopped.");
}
void executeCB(const lasa_action_planners::PLAN2CTRLGoalConstPtr &goal)
{
std::string desired_action = goal->action_type;
ROS_INFO_STREAM( "Desired Action is " << desired_action);
isOkay = false, isFTOkay = false;
ros::Rate r(10);
ROS_INFO("Waiting for EE pose/ft topic...");
while(ros::ok() && (!isOkay || !isFTOkay)) {
r.sleep();
}
if(!ros::ok()) {
result_.success = 0;
ROS_INFO("%s: Failed", action_name_.c_str());
as_.setAborted(result_);
return;
}
// initialize action progress as null
feedback_.progress = 0;
bool success = false;
///////////////////////////////////////////////
/////----- EXECUTE REQUESTED ACTION ------/////
///////////////////////////////////////////////
if (goal->action_type=="HOME"){
// TODO: do something meaningful here
tf::Pose pose(ee_pose);
success = go_home(pose);
} else if(goal->action_type == "HOME_POUR") {
// Home for pouring with lasa robot
tf::Pose pose(ee_pose);
pose.setOrigin(tf::Vector3(-0.65, -0.11, 0.474));
pose.setRotation(tf::Quaternion(-0.006, 0.907, -0.420, -0.114));
success = go_home(pose);
}
if(goal->action_type=="FIND_TABLE"){
// Go down until table found
tf::Pose pose(ee_pose);
success = go_home(ee_pose);
if(success) {
success = find_table_for_rolling(goal->height, 0.03, goal->threshold);
}
}
if(goal->action_type=="ROLL_TEST"){
/**** For now use this instead **/
tf::Pose p(ee_pose);
/*********************************/
success = go_home(p);
if(success) {
success = find_table_for_rolling(0.15, 0.03, 5);
if(success) {
success = rolling(goal->force, goal->speed, 0.1);
}
}
}
// Use learned models to do shit
if(goal->action_type=="LEARNED_MODEL"){
DoughTaskPhase phase;
if(goal->action_name == "roll") {
phase = PHASEROLL;
} else if(goal->action_name == "reach") {
phase = PHASEREACH;
} else if(goal->action_name == "back") {
phase = PHASEBACK;
} else if(goal->action_name == "home") {
phase = PHASEHOME;
} else {
ROS_ERROR_STREAM("Unidentified action name "<<goal->action_name.c_str());
result_.success = 0;
as_.setAborted(result_);
return;
}
//TODO: update these from action
double reachingThreshold = 0.02, model_dt = 0.01;
//CDSController::DynamicsType masterType = CDSController::LINEAR_DYNAMICS;
CDSController::DynamicsType masterType = CDSController::MODEL_DYNAMICS;
//CDSController::DynamicsType slaveType = CDSController::LINEAR_DYNAMICS;
CDSController::DynamicsType slaveType = CDSController::UTHETA;
tf::Transform trans_obj, trans_att;
//Little hack for using reaching phase for HOMING
if (phase==PHASEHOME){
phase=PHASEREACH;
homing = true;
}
switch (action_mode) {
case ACTION_BOXY_FIXED:
/*if(phase == PHASEREACH) {
trans_obj.setOrigin(tf::Vector3(0.7, -0.43, 0.64)); // TODO: remember to add 0.15 to tf values as well
trans_obj.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
} else if(phase == PHASEROLL) {
trans_obj.setOrigin(tf::Vector3(0.85, -0.43, ee_pose.getOrigin().z()));
trans_obj.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
} else if(phase == PHASEBACK) {
trans_obj.setOrigin(tf::Vector3(0.73, -0.63, 0.84));
trans_obj.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
}
trans_att.setIdentity();*/
trans_obj.setIdentity();
trans_obj.setOrigin(tf::Vector3(0.8, -0.43, 0.64));
if(phase == PHASEREACH) {
trans_att.setOrigin(tf::Vector3(-0.05, 0,0)); // TODO: remember to add 0.15 to tf values as well
trans_att.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
} else if(phase == PHASEROLL) {
trans_att.setOrigin(tf::Vector3(0.05, 0, 0));
trans_att.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
} else if(phase == PHASEBACK) {
trans_att.setOrigin(tf::Vector3(-0.15, -0.2, 0.15));
trans_att.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
}
break;
case ACTION_LASA_FIXED:
if(phase == PHASEREACH) {
trans_obj.setOrigin(tf::Vector3(-0.55, -0.10, 0.3)); // TODO: remember to add 0.15 to tf values as well (z was 0.15)
trans_obj.setRotation(tf::Quaternion(0.0, 1.0, 0.0, 0.0));
} else if(phase == PHASEROLL) {
trans_obj.setOrigin(tf::Vector3(-0.55, -0.25, ee_pose.getOrigin().z()));
trans_obj.setRotation(tf::Quaternion(0.0, 1.0, 0.0, 0.0));
} else if(phase == PHASEBACK) {
trans_obj.setOrigin(tf::Vector3(-0.55, -0.25, 0.3)); //(z was 0.15)
trans_obj.setRotation(tf::Quaternion(0.0, 1.0, 0.0, 0.0));
}
trans_att.setIdentity();
break;
case ACTION_VISION:
trans_obj.setRotation(tf::Quaternion(goal->object_frame.rotation.x,goal->object_frame.rotation.y,
goal->object_frame.rotation.z,goal->object_frame.rotation.w));
trans_obj.setOrigin(tf::Vector3(goal->object_frame.translation.x, goal->object_frame.translation.y,
goal->object_frame.translation.z));
trans_att.setRotation(tf::Quaternion(goal->attractor_frame.rotation.x,goal->attractor_frame.rotation.y,
goal->attractor_frame.rotation.z,goal->attractor_frame.rotation.w));
trans_att.setOrigin(tf::Vector3(goal->attractor_frame.translation.x, goal->attractor_frame.translation.y,
goal->attractor_frame.translation.z));
if (bWaitForForces){ //HACK FOR BOXY
// Hack! For setting heights for reach and add offset of roller
if(phase == PHASEREACH) {
trans_att.setOrigin(tf::Vector3(goal->attractor_frame.translation.x, goal->attractor_frame.translation.y,goal->attractor_frame.translation.z + 0.1));
}
// Hack! safety for rolling
if(phase == PHASEROLL) {
trans_obj.setOrigin(tf::Vector3(goal->object_frame.translation.x, goal->object_frame.translation.y,ee_pose.getOrigin().getZ()));
trans_att.setOrigin(tf::Vector3(goal->attractor_frame.translation.x, goal->attractor_frame.translation.y, 0.0));
}
}
/*if (bWaitForForces){ //HACK FOR LASA
// Hack! For setting heights for reach and back
if(phase == PHASEREACH || phase == PHASEBACK) {
trans_obj.getOrigin().setZ(0.15);
trans_att.getOrigin().setZ(0.0);
}
// Hack! safety for rolling
if(phase == PHASEROLL) {
trans_obj.getOrigin().setZ(ee_pose.getOrigin().getZ());
trans_att.getOrigin().setZ(0.0);
}
}*/
break;
default:
break;
}
std::string force_gmm_id = goal->force_gmm_id;
success = learned_model_execution(phase, masterType, slaveType, reachingThreshold,
model_dt, trans_obj, trans_att, base_path, force_gmm_id);
}
result_.success = success;
homing=false;
if(success)
{
if (!homing){
ROS_WARN_STREAM("STORE PLOT");
plot_dyn = 2;
plot_dyn_msg.data = plot_dyn;
pub_plot_dyn_.publish(plot_dyn_msg);
ros::Rate r(1);
r.sleep();
}
//Wait for message of "plot stored"
/*ros::Rate wait(1000);
while(ros::ok() && (plot_published!=1)) {
ros::spinOnce();
wait.sleep();
}*/
ROS_INFO("%s: Succeeded", action_name_.c_str());
as_.setSucceeded(result_);
} else {
ROS_INFO("%s: Failed", action_name_.c_str());
as_.setAborted(result_);
}
}
// Do stuff with learned models
// Phase - Reach, Roll or Back?
// Dynamics type - to select the type of master/slave dynamics (linear/model etc.)
// reachingThreshold - you know
// model_dt - you know
bool learned_model_execution(DoughTaskPhase phase, CDSController::DynamicsType masterType, CDSController::DynamicsType slaveType, double reachingThreshold, double model_dt, tf::Transform trans_obj, tf::Transform trans_att, std::string model_base_path, std::string force_gmm_id) {
ROS_INFO_STREAM(" Model Path "<<model_base_path);
ROS_INFO_STREAM(" Learned model execution with phase "<<phase);
ROS_INFO_STREAM(" Reaching threshold "<<reachingThreshold);
ROS_INFO_STREAM(" Model DT "<<model_dt);
if (force_gmm_id!="")
ROS_INFO_STREAM(" Force GMM ID: "<< force_gmm_id);
ros::Rate wait(1);
tf::Transform trans_final_target, ee_pos_att;
// To Visualize EE Frames
static tf::TransformBroadcaster br;
br.sendTransform(tf::StampedTransform(trans_final_target, ros::Time::now(), world_frame, "/attractor"));
br.sendTransform(tf::StampedTransform(trans_obj, ros::Time::now(), world_frame, "/object_frame"));
// convert attractor information to world frame
trans_final_target.mult(trans_obj, trans_att);
ROS_INFO_STREAM("Final target origin "<<trans_final_target.getOrigin().getX()<<","<<trans_final_target.getOrigin().getY()<<","<<trans_final_target.getOrigin().getZ());
ROS_INFO_STREAM("Final target orient "<<trans_final_target.getRotation().getX()<<","<<trans_final_target.getRotation().getY()<<","<<trans_final_target.getRotation().getZ()<<","<<trans_final_target.getRotation().getW());
// Publish attractors if running in simulation or with fixed values
if ((action_mode == ACTION_LASA_FIXED) || (action_mode == ACTION_BOXY_FIXED)) {
br.sendTransform(tf::StampedTransform(trans_final_target, ros::Time::now(), world_frame, "/attractor"));
}
// Initialize CDS
CDSExecution *cdsRun = new CDSExecution;
cdsRun->initSimple(model_base_path, phase, force_gmm_id);
cdsRun->setObjectFrame(toMatrix4(trans_obj));
cdsRun->setAttractorFrame(toMatrix4(trans_att));
cdsRun->setCurrentEEPose(toMatrix4(ee_pose));
cdsRun->setDT(model_dt);
if (phase==PHASEBACK || phase==PHASEROLL)
masterType = CDSController::LINEAR_DYNAMICS;
// Roll slow but move fast for reaching and back phases.
// If models have proper speed, this whole block can go!
if(phase == PHASEROLL) {
//cdsRun->setMotionParameters(1,1,0.5,reachingThreshold, masterType, slaveType);
cdsRun->setMotionParameters(1,1,2,reachingThreshold, masterType, slaveType);
// large threshold to avoid blocking forever
// TODO: should rely on preempt in action client.
// reachingThreshold = 0.02;
reachingThreshold = 0.025;
} else {
cdsRun->setMotionParameters(1,1,2,reachingThreshold, masterType, slaveType);
}
cdsRun->postInit();
// If phase is rolling, need force model as well
GMR* gmr_perr_force = NULL;
if(phase == PHASEROLL) {
gmr_perr_force = getNewGMRMappingModel(model_base_path, force_gmm_id);
if(!gmr_perr_force) {
ROS_ERROR("Cannot initialize GMR model");
return false;
}
}
ros::Duration loop_rate(model_dt);
tf::Pose mNextRobotEEPose = ee_pose;
std::vector<double> gmr_in, gmr_out;
gmr_in.resize(1);gmr_out.resize(1);
double prog_curr, full_err, pos_err, ori_err, new_err, gmr_err;
tf::Vector3 att_t, curr_t;
ROS_INFO("Execution started");
tf::Pose p;
bool bfirst = true;
std_msgs::String string_msg, action_name_msg, model_fname_msg;
std::stringstream ss, ss_model;
if(phase == PHASEREACH) {
ss << "reach ";
}
else if (phase == PHASEROLL){
ss << "roll";
}
else if (phase == PHASEBACK){
ss << "back";
}
if (!homing){
string_msg.data = ss.str();
pub_action_state_.publish(string_msg);
// ss_model << path_matlab_plot << "/Phase" << phase << "/masterGMM.fig";
if (force_gmm_id=="")
ss_model << path_matlab_plot << "/Phase" << phase << "/masterGMM.fig";
else
ss_model << path_matlab_plot << "/Phase" << phase << "/ForceProfile_" << force_gmm_id << ".fig";
//ss_model << "/Phase" << phase << "/masterGMM.fig";
model_fname_msg.data = ss_model.str();
pub_model_fname_.publish(model_fname_msg);
action_name_msg.data = ss.str();
pub_action_name_.publish(action_name_msg);
plot_dyn = 1;
plot_dyn_msg.data = plot_dyn;
pub_plot_dyn_.publish(plot_dyn_msg);
}
while(ros::ok()) {
if (!homing)
plot_dyn = 1;
else
plot_dyn = 0;
plot_dyn_msg.data = plot_dyn;
pub_plot_dyn_.publish(plot_dyn_msg);
// Nadia's stuff
// Current progress variable (position/orientation error).
// TODO: send this back to action client as current progress
pos_err = (trans_final_target.getOrigin() - ee_pose.getOrigin()).length();
//Real Orientation Error qdiff = acos(dot(q1_norm,q2_norm))*180/pi
ori_err = acos(abs(trans_final_target.getRotation().dot(ee_pose.getRotation())));
ROS_INFO_STREAM_THROTTLE(0.5,"Position Threshold : " << reachingThreshold << " ... Current Error: "<<pos_err);
ROS_INFO_STREAM_THROTTLE(0.5,"Orientation Threshold : " << 0.01 << " ... Current Error: "<<ori_err);
/*double att_pos_err = (trans_final_target.getOrigin() - p.getOrigin()).length();
double att_ori_err = acos(abs(trans_final_target.getRotation().dot(p.getRotation())));
ROS_INFO_STREAM_THROTTLE(0.5,"Des-Att Position Error: " << att_pos_err);
ROS_INFO_STREAM_THROTTLE(0.5,"Des-Att Orientation Error: " << att_ori_err);*/
switch (phase) {
// Home, reach and back are the same control-wise
case PHASEREACH:
case PHASEBACK:
// Current progress variable (position/orientation error).
// TODO: send this back to action client as current progress
prog_curr = 0.5*((trans_final_target.getOrigin() - ee_pose.getOrigin()).length() + (trans_final_target.getRotation()-ee_pose.getRotation()).length());
// Compute Next Desired EE Pose
cdsRun->setCurrentEEPose(toMatrix4(mNextRobotEEPose));
toPose(cdsRun->getNextEEPose(), mNextRobotEEPose);
p = mNextRobotEEPose;
// Aswhini's Hack! Dont rely on model's orientation interpolation. Set it equal to target orientation to avoid
// going the wrong way around
p.setRotation(trans_final_target.getRotation());
//Publish desired force
gmr_msg.data = 0.0;
pub_gmr_out_.publish(gmr_msg);
break;
case PHASEROLL:
// Current progress in rolling phase is simply the position error
prog_curr = (trans_final_target.getOrigin() - ee_pose.getOrigin()).length();
// New position error being fed to GMR Force Model
att_t = tf::Vector3(trans_final_target.getOrigin().getX(),trans_final_target.getOrigin().getY(),0.0);
curr_t = tf::Vector3(ee_pose.getOrigin().getX(),ee_pose.getOrigin().getY(),0.0);
new_err = (att_t - curr_t).length();
gmr_err = new_err;
//Hack! Truncate errors to corresponding models
if ((strncmp(force_gmm_id.c_str(),"first",5)==0) && (new_err > 0.03)){
gmr_err = 0.03;
}
if ((strncmp(force_gmm_id.c_str(),"mid",3)==0) && (new_err > 0.07)){
gmr_err = 0.07;
}
if ((strncmp(force_gmm_id.c_str(),"last",4)==0) && (new_err > 0.13)){
gmr_err = 0.13;
}
//pos_err = prog_curr;
ori_err = 0;
gmr_err = gmr_err;
gmr_in[0] = gmr_err; // distance between EE and attractor
// Query the model for desired force
getGMRResult(gmr_perr_force, gmr_in, gmr_out);
/* double fz_plot;
getGMRResult(gmr_perr_force, -gmr_in, fz_plot);*/
//-> INSTEAD OF SENDING GMR OUTPUT / SEND EST_EE_FT(Z)
// Send fz and distance to attractor for plotting
msg_ft.wrench.force.x = gmr_err;
msg_ft.wrench.force.y = gmr_out[0]; //ee_ft[2]
msg_ft.wrench.force.z = 0;
msg_ft.wrench.torque.x = 0;
msg_ft.wrench.torque.y = 0;
msg_ft.wrench.torque.z = 0;
pub_ee_ft_att_.publish(msg_ft);
// Hack! Scale the force to be in reasonable values
gmr_out[0] = FORCE_SCALING*fabs(gmr_out[0]);
ROS_INFO_STREAM_THROTTLE(0.5,"Distance to Attractor: " << new_err << " GMR output (N): " << gmr_out[0]);
gmr_msg.data = gmr_out[0];
pub_gmr_out_.publish(gmr_msg);
// Hack! Safety first!
if(gmr_out[0] > MAX_ROLLING_FORCE) {
gmr_out[0] = MAX_ROLLING_FORCE;
}
// Give some time for the force to catch up the first time. Then roll with constant force thereafter.
if(bfirst) {
sendAndWaitForNormalForce(-gmr_out[0]);
bfirst = false;
} else {
sendNormalForce(-gmr_out[0]);
}
ROS_INFO_STREAM_THROTTLE(0.5, "Force applied: "<<gmr_out[0]);
cdsRun->setCurrentEEPose(toMatrix4(mNextRobotEEPose));
toPose(cdsRun->getNextEEPose(), mNextRobotEEPose);
p = mNextRobotEEPose;
// Hack! Dont rely on model orientation. Use target orientation instead.
p.setRotation(trans_final_target.getRotation());
// Hack! Dont rely on the Z component of the model. It might go below the table!
p.getOrigin().setZ(trans_final_target.getOrigin().getZ());
homing=false;
break;
default:
ROS_ERROR_STREAM("No such phase defined "<<phase);
return false;
}
// Add rotation of Tool wrt. flange_link for BOXY
/*if (use_boxy_tool){
tf::Matrix3x3 R = p.getBasis();
Eigen::Matrix3d R_ee;
tf::matrixTFToEigen(R,R_ee);
Eigen::Matrix3d R_tool;
R_tool << -0.7071, -0.7071, 0.0,
0.7071,-0.7071, 0.0,
0.0, 0.0, 1.0;
//multiply tool rot
R_ee = R_ee*R_tool;
tf::matrixEigenToTF(R_ee,R);
p.setBasis(R);
}*/
// Send the computed pose from one of the above phases
sendPose(p);
// convert and send ee pose to attractor frame to plots
ee_pos_att.mult(trans_final_target.inverse(), p);
geometry_msgs::PoseStamped msg;
msg.pose.position.x = ee_pos_att.getOrigin().x();
msg.pose.position.y = ee_pos_att.getOrigin().y();
msg.pose.position.z = ee_pos_att.getOrigin().z();
msg.pose.orientation.x = ee_pos_att.getRotation().x();
msg.pose.orientation.y = ee_pos_att.getRotation().y();
msg.pose.orientation.z = ee_pos_att.getRotation().z();
msg.pose.orientation.w = ee_pos_att.getRotation().w();
pub_ee_pos_att_.publish(msg);
//ROS_INFO_STREAM_THROTTLE(0.5,"Error "<<prog_curr);
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
sendPose(ee_pose);
sendNormalForce(0);
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
return false;
}
feedback_.progress = prog_curr;
as_.publishFeedback(feedback_);
/* if(prog_curr < reachingThreshold) {
break;
}*/
//Orientation error 0.05
if(pos_err < reachingThreshold && (ori_err < 0.05 || isnan(ori_err))) {
break;
}
loop_rate.sleep();
}
delete cdsRun;
if(phase == PHASEREACH) {
// Hack! If phase is "reach", find the table right after reaching
if (bWaitForForces && !homing) {
bool x = find_table_for_rolling(0.35, 0.05, 5);
// bool x = find_table_for_rolling(0.35, 0.1, 5);
//-> Send command to dynamics plotter to stop logging
return x;
}
if (!homing){
//-> Send command to dynamics plotter to stop logging
}
} else if (phase == PHASEROLL){
// Hack! wait for zero force before getting ready to recieve further commands.
// This is to avoid dragging the dough.
sendAndWaitForNormalForce(0);
//-> Send command to dynamics plotter to start plotting
return ros::ok();
} else {
//->Send command to dynamics plotter to start plotting
return ros::ok();
}
}
/**
* Convert tf::Pose to string
*/
template<> std::string toString<tf::Pose>(const tf::Pose& p_pose)
{
std::stringstream ss;
ss << "Pose(Quaternion=" << toString(p_pose.getRotation()) << "; Vector3=" << toString(p_pose.getOrigin()) << ")";
return ss.str();
}
void
LoopClosure::addNode(const tf::Pose& pose)
{
bool add = false;
// If there are no nodes yet, add one now
if(curr_node_ == NULL)
add = true;
else
{
geometry_msgs::PoseStamped planner_start;
geometry_msgs::Pose temp_pose;
tf::poseTFToMsg(pose, temp_pose);
planner_start.header.stamp = ros::Time::now();
planner_start.header.frame_id = costmap_.getGlobalFrameID();
planner_start.pose = temp_pose;
//we'll only compute the potential once from our position
planner_->computePotential(planner_start.pose.position);
add = true;
// How far are we from the closest node?
double mind = DBL_MAX;
GraphNode* closest_node = NULL;
for(int i = nodes_.size() - 1; i >= 0; --i)
{
GraphNode* index_node = nodes_[i];
//we need to convert from tf to message types for the planner
geometry_msgs::PoseStamped planner_end;
tf::poseTFToMsg(index_node->pose_, temp_pose);
planner_end.header.stamp = ros::Time::now();
planner_end.header.frame_id = costmap_.getGlobalFrameID();
planner_end.pose = temp_pose;
//now we'll make a plan from one point to the other
std::vector<geometry_msgs::PoseStamped> plan;
bool valid_plan = planner_->getPlanFromPotential(planner_end, plan) && !plan.empty();
// Graph distance check satisfied; compute map distance
if(valid_plan){
double path_length = 0.0;
//compute the path length
if(plan.size() > 1){
//double dist = distance(plan[0], plan[1]);
//path_length = dist * (plan.size() - 1);
for(unsigned int j = 0; j < plan.size() - 1; ++j){
double dist = distance(plan[j], plan[j + 1]);
path_length += dist;
}
if(path_length < mind){
mind = path_length;
closest_node = index_node;
//we'll also update our current node here because we always want to add ourselves as a node to the
//closest node
curr_node_ = closest_node;
}
}
}
}
if(mind < addition_dist_min_)
{
// We found a close-enough node, so we won't add one, unless we lost visibility
add = false;
/*
for(int i = nodes_.size() - 1; i >= 0; --i)
{
GraphNode* index_node = nodes_[i];
//we also need to check if we have visibility from our current node to the previous node
bool is_visible = true;
VisibilityChecker checker(visibility_map.getCharMap(), is_visible);
//get map coordinates
bool in_bounds = true;
unsigned int x0, y0;
if(!visibility_map.worldToMap(pose.getOrigin().x(), pose.getOrigin().y(), x0, y0)){
ROS_WARN("Attempting to check visibility from a point off the map... this should never happen");
in_bounds = false;
}
unsigned int x1, y1;
if(!visibility_map.worldToMap(index_node->pose_.getOrigin().x(), index_node->pose_.getOrigin().y(), x1, y1)){
ROS_WARN("Attempting to check visibility to a point off the map... this should never happen");
in_bounds = false;
}
if(in_bounds){
//raytrace a line with our visibility checker
raytraceLine(checker, x0, y0, x1, y1, visibility_map.getSizeInCellsX());
//check if we have visibility to the node
if(is_visible){
add = false;
break;
}
}
}
*/
// Update current node to be this one.
curr_node_ = closest_node;
// Store the current slam entropy; we compare to it later to decide
// to terminate loop closure.
if(slam_entropy_ > 0.0)
curr_node_->slam_entropy_ = slam_entropy_;
}
}
if(add)
{
ROS_INFO("Adding node at %.3f, %.3f", pose.getOrigin().x(), pose.getOrigin().y());
GraphNode* n = new GraphNode(nodes_.size(), pose);
nodes_.push_back(n);
// Add an empty adjacency list for the new node
std::vector<int> edges;
graph_.push_back(edges);
if(curr_node_)
{
// Update both adjacency lists
graph_[curr_node_->id_].push_back(n->id_);
graph_[n->id_].push_back(curr_node_->id_);
curr_node_->next_ = n;
}
curr_node_ = n;
// Store the current slam entropy; we compare to it later to decide
// to terminate loop closure.
if(slam_entropy_ > 0.0)
curr_node_->slam_entropy_ = slam_entropy_;
}
}
