void poseKDLToTF(const KDL::Frame& k, tf::Pose& t)
{
t.setOrigin(tf::Vector3(k.p[0], k.p[1], k.p[2]));
t.setBasis(tf::Matrix3x3(k.M.data[0], k.M.data[1], k.M.data[2],
k.M.data[3], k.M.data[4], k.M.data[5],
k.M.data[6], k.M.data[7], k.M.data[8]));
}
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];
}
// 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 toPose(const MathLib::Matrix4& mat4, tf::Pose& pose) {
MathLib::Matrix3 m1 = mat4.GetOrientation();
MathLib::Vector3 v1 = m1.GetRotationAxis();
tf::Vector3 ax(v1(0), v1(1), v1(2));
pose.setRotation(tf::Quaternion(ax, m1.GetRotationAngle()));
v1.Set(mat4.GetTranslation());
pose.setOrigin(tf::Vector3(v1(0),v1(1),v1(2)));
}
bool MarkerSearcherComputation::getNextSearchPose(tf::Pose& returnPose) {
if(poseReturned) {
return false;
}
returnPose.setOrigin(nextSearchPose.getOrigin());
returnPose.setRotation(nextSearchPose.getRotation());
poseReturned = true;
return true;
}
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;
}
void toTfPose(const geometry_msgs::Pose& msg_pose, tf::Pose& tf_pose)
{
tf_pose.setOrigin(tf::Vector3(msg_pose.position.x,
msg_pose.position.y,
msg_pose.position.z));
tf_pose.setRotation(tf::Quaternion(msg_pose.orientation.x,
msg_pose.orientation.y,
msg_pose.orientation.z,
msg_pose.orientation.w));
}
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;
}
// 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);
}
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 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;
}
// 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());
}
}
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);
}
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;
}
// 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);
}
pair<Nodes, PointVec>
nodesOnGrid (const unsigned g, const Roadmap& r,
const tmap::TopologicalMap& tmap)
{
pair<Nodes, PointVec> res;
// Add all nodes defined wrt this grid
BOOST_FOREACH (const unsigned n, r.gridNodes(g))
{
res.first.push_back(n);
res.second.push_back(r.nodeInfo(n).position);
}
const double x_size = tmap.nodeInfo(g).x_size;
const double y_size = tmap.nodeInfo(g).y_size;
// Also look at neighboring grids in topological graph
BOOST_FOREACH (const tmap::GraphEdge e,
out_edges(tmap.node(g), tmap))
{
const bool flip = (tmap[e].dest == g);
const tf::Pose offset = gmu::toPose(tmap[e].offset);
const tf::Transform tr = flip ? offset.inverse() : offset;
const unsigned g2 = flip ? tmap[e].src : tmap[e].dest;
// Now tr maps from the neighbor grid g2 to g
BOOST_FOREACH (const unsigned n, r.gridNodes(g2))
{
ROS_ASSERT(g2==r.nodeInfo(n).grid);
const gm::Point& pos = r.nodeInfo(n).position;
const gm::Point local_pos = gmu::transformPoint(tr, pos);
// Now local_pos is the position of the waypoint in frame g
ROS_DEBUG_STREAM_NAMED ("nodes_on_grid", "Node " << n << " position on " << g2 << " is "
<< gmu::toString(pos) << " and on " << g <<
" is " << gmu::toString(local_pos));
if (fabs(local_pos.x) < x_size/2 && fabs(local_pos.y) < y_size/2)
{
ROS_DEBUG_NAMED ("nodes_on_grid", "On grid");
res.first.push_back(n);
res.second.push_back(local_pos);
}
}
}
return res;
}
void RTKRobotArm::getEEPose(tf::Pose& pose) {
if(mRobot) {
//Collect ee pose from RTK
MathLib::Vector eeq(4);
Matrix3 tmp = mRobot->GetReferenceFrame(nEndEffectorId).GetOrient();
tmp.GetQuaternionRepresentation(eeq);
MathLib::Vector3 eep = mRobot->GetReferenceFrame(nEndEffectorId).GetOrigin();
// Convert to tf
pose.setRotation(tf::Quaternion(eeq[1], eeq[2], eeq[3], eeq[0]));
pose.setOrigin(tf::Vector3(eep[0], eep[1], eep[2]));
} else{
ROS_WARN("RTK Robot not initialized");
}
}
double
AmclNode::getYaw(tf::Pose& t)
{
double yaw, pitch, roll;
t.getBasis().getEulerYPR(yaw,pitch,roll);
return yaw;
}
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);
}
double
AmclNode::getYaw(tf::Pose& t)
{
double yaw, pitch, roll;
btMatrix3x3 mat = t.getBasis();
mat.getEulerYPR(yaw,pitch,roll);
return yaw;
}
/**
* 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();
}
bool OdometryRemap::setOdomPoseCallback(nao_remote::SetTransform::Request& req, nao_remote::SetTransform::Response& res){
ROS_INFO("New target for current odometry pose received");
tf::Transform targetPose;
tf::transformMsgToTF(req.offset, targetPose);
m_odomOffset = targetPose * m_odomPose.inverse();
return true;
}
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;
}
// 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;
}
// 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();
}
gm::Point positionOnGrid (const unsigned n, const unsigned g, const Roadmap& r,
const tmap::TopologicalMap& tmap)
{
const msg::RoadmapNode& info = r.nodeInfo(n);
const unsigned g2 = info.grid;
if (g2==g)
{
return info.position;
}
else
{
const tmap::GraphVertex v = tmap.node(g);
const tmap::GraphVertex v2 = tmap.node(g2);
pair<tmap::GraphEdge, bool> res = edge(v, v2, tmap);
ROS_ASSERT_MSG (res.second, "No edge between grid %u and grid %u "
"containing %u", g, g2, n);
const bool flip = (tmap[res.first].dest == g);
const tf::Pose offset = gmu::toPose(tmap[res.first].offset);
const tf::Transform tr = flip ? offset.inverse() : offset;
return gmu::transformPoint(tr, info.position);
}
}
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;
}
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;
}
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;
}