void Robot::updateFrom(tf::TransformListener *tfListener) {
using namespace Ogre;
static tf::StampedTransform baseTF;
static Vector3 translation = Vector3::ZERO;
static Quaternion orientation = Quaternion::IDENTITY;
static Quaternion qRot = Quaternion(-sqrt(0.5), 0.0f, sqrt(0.5), 0.0f)*Quaternion(-sqrt(0.5), sqrt(0.5), 0.0f, 0.0f);
static Quaternion qYn90 = Quaternion(Degree(90), Vector3::NEGATIVE_UNIT_Y);
static tfScalar yaw,pitch,roll;
static Matrix3 mRot;
try {
tfListener->lookupTransform("map","base_footprint",ros::Time(0), baseTF);
translation.x = baseTF.getOrigin().x();
translation.y = baseTF.getOrigin().y();
translation.z = baseTF.getOrigin().z();
translation = qRot*translation + Vector3(0.0f, 1.0f, 0.0f);
baseTF.getBasis().getEulerYPR(yaw,pitch,roll);
mRot.FromEulerAnglesYXZ(Radian(yaw),Radian(0.0f),Radian(0.0f));
orientation.FromRotationMatrix(mRot);
orientation = qYn90*orientation;
robot->setPosition(translation);
robot->setOrientation(orientation);
} catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
}
std::vector<desperate_housewife::fittedGeometriesSingle> HandPoseGenerator::SortbyHand(std::vector<desperate_housewife::fittedGeometriesSingle> objects_vec, tf::StampedTransform hand_position)
{
geometry_msgs::Pose hand_position_local;
hand_position_local.position.x = hand_position.getOrigin().x();
hand_position_local.position.y = hand_position.getOrigin().y();
hand_position_local.position.z = hand_position.getOrigin().z();
hand_position_local.orientation.x = hand_position.getRotation().x();
hand_position_local.orientation.y = hand_position.getRotation().y();
hand_position_local.orientation.z = hand_position.getRotation().z();
hand_position_local.orientation.w = hand_position.getRotation().w();
Eigen::Matrix4d T_h_w = FromMsgtoEigen(hand_position_local);
std::vector< desperate_housewife::fittedGeometriesSingle > objects_vec_roted_by_hand = objects_vec;
for (unsigned int i = 0; i < objects_vec.size(); i++)
{
Eigen::Matrix4d T_o_w = FromMsgtoEigen(objects_vec_roted_by_hand[i].pose);
geometry_msgs::Pose pose_temp;
Eigen::Matrix4d T_o_h = T_h_w.inverse() * T_o_w;
fromEigenToPose(T_o_h, pose_temp);
// desperate_housewife::fittedGeometriesSingle object_temp = objects_vec[i];
objects_vec_roted_by_hand[i].pose = pose_temp;
// objects_vec_roted_by_hand.push_back(object_temp);
}
std::sort(objects_vec_roted_by_hand.begin(), objects_vec_roted_by_hand.end(), [](desperate_housewife::fittedGeometriesSingle first, desperate_housewife::fittedGeometriesSingle second) {
double distfirst = std::sqrt( first.pose.position.x * first.pose.position.x + first.pose.position.y * first.pose.position.y + first.pose.position.z * first.pose.position.z);
double distsecond = std::sqrt( second.pose.position.x * second.pose.position.x + second.pose.position.y * second.pose.position.y + second.pose.position.z * second.pose.position.z);
return (distfirst < distsecond);
});
return objects_vec_roted_by_hand;
}
gazebo::math::Pose gazebo::IAI_BoxDocking::tfToGzPose(tf::StampedTransform transform)
{
math::Quaternion rotation = math::Quaternion( transform.getRotation().w(), transform.getRotation().x(), transform.getRotation().y(), transform.getRotation().z());
math::Vector3 origin = math::Vector3(transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z()); //T_q^w
math::Pose pose = math::Pose(origin,rotation);
return pose;
}
/*! \brief set the class attribut */
void Segment::setSegmentAttribut(tf::StampedTransform distal_transform_,tf::StampedTransform proximal_transform_,string segment_id_)
{
this->proximal_joint.setValue(proximal_transform_.getOrigin().x(),proximal_transform_.getOrigin().y(),proximal_transform_.getOrigin().z());
this->distal_joint.setValue(distal_transform_.getOrigin().x(),distal_transform_.getOrigin().y(),distal_transform_.getOrigin().z()) ;
this->vec_seg = -(this->proximal_joint - this->distal_joint);
this->vec_uni = this->vec_seg.normalized() ;
if(this->vec_uni.getX() != 0) this->p_limite = this->vec_seg.getX()/this->vec_uni.getX();
this->segment_id = segment_id_;
//-------------------------> affichage des variables du segment
/*
std::cout <<"prox_x =" << proximal_transform_.getOrigin().x() << std::endl;
std::cout <<"prox_y =" << proximal_transform_.getOrigin().y() << std::endl;
std::cout <<"prox_z =" << proximal_transform_.getOrigin().z() << std::endl;
std::cout <<"dist_x =" << distal_transform_.getOrigin().x() << std::endl;
std::cout <<"dist_y =" << distal_transform_.getOrigin().y() << std::endl;
std::cout <<"dist_z =" << distal_transform_.getOrigin().z() << std::endl;
std::cout <<"vec_seg = (" << vec_seg.x() << ";" << vec_seg.y() << ";" << vec_seg.z() << ")" << std::endl;
std::cout <<"vec_uni = (" << vec_uni.x() << ";" << vec_uni.y() << ";" << vec_uni.z() << ")" << std::endl;
std::cout <<"p_limite =" << this->vec_seg.getX()/this->vec_uni.getX() << std::endl;
std::cout <<"segment_id =" << segment_id << std::endl;
*/
//------------------------------------------------------------------------------------------------------------
}
void computeFrustum()
{
// compute frustum based on camera info and tf
const double alphaY = cameraInfo.K[4];
const double fovY = 2 * atan(cameraInfo.height / (2 * alphaY));
tf::Vector3 up(0, 1, 0), focal(1, 0, 0);
up = camToWorld * up;
focal = camToWorld * focal;
camera.fovy = fovY;
camera.clip[0] = 0.001;
camera.clip[1] = 12.0;
camera.window_size[0] = cameraInfo.width;
camera.window_size[1] = cameraInfo.height;
camera.window_pos[0] = 0;
camera.window_pos[1] = 0;
camera.pos[0] = worldToCam.getOrigin().x();
camera.pos[1] = worldToCam.getOrigin().y();
camera.pos[2] = worldToCam.getOrigin().z();
camera.view[0] = up.x();
camera.view[1] = up.y();
camera.view[2] = up.z();
camera.focal[0] = focal.x();
camera.focal[1] = focal.y();
camera.focal[2] = focal.z();
Eigen::Matrix4d viewMatrix;
Eigen::Matrix4d projectionMatrix;
Eigen::Matrix4d viewProjectionMatrix;
camera.computeViewMatrix(viewMatrix);
camera.computeProjectionMatrix(projectionMatrix);
viewProjectionMatrix = projectionMatrix * viewMatrix;
pcl::visualization::getViewFrustum(viewProjectionMatrix, frustum);
}
void InputOutputLinControl::getRelativeTransformation2D(tf::StampedTransform v1, tf::StampedTransform v2, tf::Transform& relative_transform)
{
double roll, pitch, yaw1, yaw2;
v1.getBasis().getRPY(roll,pitch,yaw1);
v2.getBasis().getRPY(roll,pitch,yaw2);
double a11 = cos(yaw1 - yaw2);
double a12 = sin(yaw1 - yaw2);
double a13 = 0;
double a21 = - a12;
double a22 = a11;
double a23 = 0;
double a31 = 0;
double a32 = 0;
double a33 = 1;
double t1 = v2.getOrigin().getX() - v1.getOrigin().getX() * a11 - v1.getOrigin().getY()*a12;
double t2 = v2.getOrigin().getY() - v1.getOrigin().getY() * a11 + v1.getOrigin().getX()*a12;
double t3 = 0;
relative_transform = tf::Transform(btMatrix3x3(a11,a12,a13,a21,a22,a23,a31,a32,a33),btVector3(t1,t2,t3));
}
std::vector<desperate_housewife::fittedGeometriesSingle> HandPoseGenerator::transform_to_world(std::vector<desperate_housewife::fittedGeometriesSingle> objects_vec, tf::StampedTransform hand_position)
{
geometry_msgs::Pose hand_position_local;
hand_position_local.position.x = hand_position.getOrigin().x();
hand_position_local.position.y = hand_position.getOrigin().y();
hand_position_local.position.z = hand_position.getOrigin().z();
hand_position_local.orientation.x = hand_position.getRotation().x();
hand_position_local.orientation.y = hand_position.getRotation().y();
hand_position_local.orientation.z = hand_position.getRotation().z();
hand_position_local.orientation.w = hand_position.getRotation().w();
Eigen::Matrix4d T_h_w = FromMsgtoEigen(hand_position_local);
std::vector< desperate_housewife::fittedGeometriesSingle > objects_vec_roted_by_hand = objects_vec;
for (unsigned int p = 0; p < objects_vec_roted_by_hand.size(); p ++)
{
geometry_msgs::Pose pose_temp;
Eigen::Matrix4d T_o_h = FromMsgtoEigen(objects_vec_roted_by_hand[p].pose);
Eigen::Matrix4d Temp = T_h_w * T_o_h;
fromEigenToPose(Temp, pose_temp);
objects_vec_roted_by_hand[p].pose = pose_temp;
}
return objects_vec_roted_by_hand;
}
void InputOutputLinControl::markerFromPose(std::string name_space, double red, double green, double blue, tf::StampedTransform pose, visualization_msgs::Marker& marker)
{
marker.header.frame_id = pose.frame_id_;
marker.header.stamp = pose.stamp_;
marker.ns = name_space.c_str();
marker.id = 0;
marker.type = visualization_msgs::Marker::ARROW;
marker.action = visualization_msgs::Marker::ADD;
marker.pose.position.x = pose.getOrigin().getX();
marker.pose.position.y = pose.getOrigin().getY();
marker.pose.position.z = pose.getOrigin().getZ();
marker.pose.orientation.x = pose.getRotation().getX();
marker.pose.orientation.y = pose.getRotation().getY();
marker.pose.orientation.z = pose.getRotation().getZ();
marker.pose.orientation.w = pose.getRotation().getW();
marker.scale.x = 1;
marker.scale.y = 1;
marker.scale.z = 0.8;
marker.color.a = 1;
marker.color.g = green;
marker.color.r = red;
marker.color.b = blue;
marker.lifetime = ros::Duration(0);
}
geometry_msgs::PoseStamped getCartBaseLinkPosition()
{
geometry_msgs::PoseStamped pose_base_link;
pose_base_link.header.frame_id = base_link_;
while (nh_.ok()) {
try {
tf_listener_.lookupTransform(base_link_, lwr_tip_link_, ros::Time(0), base_link_transform_);
pose_base_link.pose.position.x = base_link_transform_.getOrigin().x();
pose_base_link.pose.position.y = base_link_transform_.getOrigin().y();
pose_base_link.pose.position.z = base_link_transform_.getOrigin().z();
pose_base_link.pose.orientation.x = base_link_transform_.getRotation().x();
pose_base_link.pose.orientation.y = base_link_transform_.getRotation().y();
pose_base_link.pose.orientation.z = base_link_transform_.getRotation().z();
pose_base_link.pose.orientation.w = base_link_transform_.getRotation().w();
return pose_base_link;
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
ros::Duration(0.1).sleep();
}
}
}
/* ! \brief with transforms without segment_id */
Segment::Segment(tf::StampedTransform proximal_transform_,tf::StampedTransform distal_transform_)
{
this->proximal_joint.setValue(proximal_transform_.getOrigin().x(),proximal_transform_.getOrigin().y(),proximal_transform_.getOrigin().z());
this->distal_joint.setValue(distal_transform_.getOrigin().x(),distal_transform_.getOrigin().y(),distal_transform_.getOrigin().z()) ;
this->vec_seg = -(proximal_joint-distal_joint);
this->vec_uni = this->vec_seg.normalized() ;
if(this->vec_uni.getX() != 0) this->p_limite = this->vec_seg.getX()/this->vec_uni.getX();
}
cv::Mat transform2mat (tf::StampedTransform transform) {
double x = transform.getOrigin().x();
double y = transform.getOrigin().y();
double z = transform.getOrigin().z();
tf::Matrix3x3 R(transform.getRotation());
Mat P = (Mat_<float>(4,4) << R[0][0], R[0][1], R[0][2], x,
R[1][0], R[1][1], R[1][2], y,
R[2][0], R[2][1], R[2][2], z,
0, 0, 0, 1);
return P;
}
void transform_callback(tf::StampedTransform transform){
double roll,pitch,yaw;
Pos_Controller_U.position[0]=transform.getOrigin().x();
Pos_Controller_U.position[1]=transform.getOrigin().y();
Pos_Controller_U.position[2]=transform.getOrigin().z();
tf::Matrix3x3(transform.getRotation()).getRPY(roll,pitch,yaw);
Pos_Controller_U.yaw=yaw;
ROS_INFO("new position : %f %f %f %f",Pos_Controller_U.position[0],Pos_Controller_U.position[1],Pos_Controller_U.position[2], yaw);
}
void InputOutputLinControl::updateMarkerFromPose(tf::StampedTransform pose, visualization_msgs::Marker& marker)
{
marker.header.stamp = pose.stamp_;
marker.header.frame_id = pose.frame_id_;
marker.pose.position.x = pose.getOrigin().getX();
marker.pose.position.y = pose.getOrigin().getY();
marker.pose.position.z = pose.getOrigin().getZ();
marker.pose.orientation.x = pose.getRotation().getX();
marker.pose.orientation.y = pose.getRotation().getY();
marker.pose.orientation.z = pose.getRotation().getZ();
marker.pose.orientation.w = pose.getRotation().getW();
}
void convertTFtoVispHMat(const tf::StampedTransform &TFtransform,
vpHomogeneousMatrix &HMatrix)
{
double Angle; // Theta U angle of the transform
Angle = TFtransform.getRotation().getAngle();
HMatrix.buildFrom(TFtransform.getOrigin().x(),
TFtransform.getOrigin().y(),
TFtransform.getOrigin().z(),
Angle*TFtransform.getRotation().getAxis().x(),
Angle*TFtransform.getRotation().getAxis().y(),
Angle*TFtransform.getRotation().getAxis().z()
);
}
// compute nav fn (using wavefront assumption)
void getNavFn(double* cost_map, tf::StampedTransform &robot_to_map_transform, double* nav_fn) {
Point robot_pt(robot_to_map_transform.getOrigin().getX() / MAP_RESOLUTION,robot_to_map_transform.getOrigin().getY() / MAP_RESOLUTION);
Point goal_pt(goal_pose.getOrigin().getX() / MAP_RESOLUTION, goal_pose.getOrigin().getY() / MAP_RESOLUTION);
ROS_INFO("[getNavFn]\trobot_pt: (%d,%d), goal_pt(%d,%d)",robot_pt.x,robot_pt.y,goal_pt.x,goal_pt.y);
// setup init map
bool init_set_map[MAP_CELLS];
for (int i=0; i<MAP_CELLS;i++) {
init_set_map[i] = false;
}
setVal(init_set_map,goal_pt.x,goal_pt.y,true);
//updateMapDijkstra(nav_fn, init_set_map, goal_pt, cost_map, INT_MAX);
LPN2(nav_fn, cost_map, goal_pt, robot_pt);
}
bool poseTracker::newPose(tf::StampedTransform transform){
Eigen::Vector3f curr(transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z());
pose.push(curr);
if(pose.size() < 50){
return true;
}
float squaredNorm = (pose.front() - curr).squaredNorm();
pose.pop();
bool tmp = squaredNorm > 0.2;
std::cout << tmp << " Max: " << squaredNorm << std::endl;
return squaredNorm > 0.2;
}
// Calculating and returning the center2center transform which belongs to stampedT_in
// [ A(front), B(right), C(back) or D(left) ]
tf::StampedTransform C2C_LEFT_Node::get_c2c(const tf::StampedTransform& stampedT_in)
{
tf::Transform T_in = tf::Transform(stampedT_in.getRotation(), stampedT_in.getOrigin());
if (stampedT_in.child_frame_id_[1] != '_') // if stampedT_in was stand-alone (previously 1 transforms)
{
switch (stampedT_in.child_frame_id_[1] - '0')
{
case 0: case 1: return tf::StampedTransform(apollon[1]*T_in*boreas[3].inverse(), ros::Time::now(), "apollon_center", "A_" + side + "_c2c");
case 2: case 3: return tf::StampedTransform(apollon[1]*T_in*boreas[4].inverse()*boreas[3].inverse(), ros::Time::now(), "apollon_center", "B_" + side + "_c2c");
case 4: case 5: return tf::StampedTransform(apollon[1]*T_in*boreas[5].inverse()*boreas[3].inverse(), ros::Time::now(), "apollon_center", "C_" + side + "_c2c");
case 6: case 7: return tf::StampedTransform(apollon[1]*T_in*boreas[6].inverse()*boreas[3].inverse(), ros::Time::now(), "apollon_center", "D_" + side + "_c2c");
}
}
else // else stampedT_in was merged (previously 2 transforms)
{
if (stampedT_in.child_frame_id_[0] == 'A')
return tf::StampedTransform(apollon[1]*T_in*boreas[3].inverse(), ros::Time::now(), "apollon_center", "A_" + side + "_c2c");
else if (stampedT_in.child_frame_id_[0] == 'B')
return tf::StampedTransform(apollon[1]*T_in*boreas[4].inverse()*boreas[3].inverse(), ros::Time::now(), "apollon_center", "B_" + side + "_c2c");
else if (stampedT_in.child_frame_id_[0] == 'C')
return tf::StampedTransform(apollon[1]*T_in*boreas[5].inverse()*boreas[3].inverse(), ros::Time::now(), "apollon_center", "C_" + side + "_c2c");
else if (stampedT_in.child_frame_id_[0] == 'D')
return tf::StampedTransform(apollon[1]*T_in*boreas[6].inverse()*boreas[3].inverse(), ros::Time::now(), "apollon_center", "D_" + side + "_c2c");
}
}
bool HectorMappingRos::rosPointCloudToDataContainer(const sensor_msgs::PointCloud& pointCloud, const tf::StampedTransform& laserTransform, hectorslam::DataContainer& dataContainer, float scaleToMap)
{
size_t size = pointCloud.points.size();
dataContainer.clear();
tf::Vector3 laserPos (laserTransform.getOrigin());
dataContainer.setOrigo(Eigen::Vector2f(laserPos.x(), laserPos.y())*scaleToMap);
for (size_t i = 0; i < size; ++i)
{
const geometry_msgs::Point32& currPoint(pointCloud.points[i]);
float dist_sqr = currPoint.x*currPoint.x + currPoint.y* currPoint.y;
if ( (dist_sqr > p_sqr_laser_min_dist_) && (dist_sqr < p_sqr_laser_max_dist_) ){
if ( (currPoint.x < 0.0f) && (dist_sqr < 0.50f)){
continue;
}
tf::Vector3 pointPosBaseFrame(laserTransform * tf::Vector3(currPoint.x, currPoint.y, currPoint.z));
float pointPosLaserFrameZ = pointPosBaseFrame.z() - laserPos.z();
if (pointPosLaserFrameZ > p_laser_z_min_value_ && pointPosLaserFrameZ < p_laser_z_max_value_)
{
dataContainer.add(Eigen::Vector2f(pointPosBaseFrame.x(),pointPosBaseFrame.y())*scaleToMap);
}
}
}
return true;
}
bool
WmObjectCapture::updateObjectFrame(const ros::Time& stamp, tf::StampedTransform& m2c)
{
std::vector<tf::StampedTransform> valids;
getValidMarks(valids, stamp);
if(valids.size()<4)
{
//ROS_INFO("No enough marks detected [%zu]", valids.size());
return false;
}
tf::Transform media;
double stdev;
getBestTransform(valids, media, stdev);
double m_tolerance2 = 0.006*0.006;
if(stdev<m_tolerance2)
{
//ROS_INFO("TRANSFORM ACCEPTED ");
m2c.child_frame_id_ = objectFrameId_;
m2c.frame_id_ = cameraFrameId_;
m2c.stamp_ = ros::Time::now();
m2c.setOrigin(media.getOrigin());
m2c.setRotation(media.getRotation());
ROS_DEBUG("NEW FINAL (%lf, %lf, %lf) (%lf, %lf, %lf, %lf)",
m2c.getOrigin().x(), m2c.getOrigin().y(), m2c.getOrigin().z(),
m2c.getRotation().x(), m2c.getRotation().y(), m2c.getRotation().z(), m2c.getRotation().w());
try
{
tfBroadcaster_.sendTransform(m2c);
}catch(tf::TransformException & ex)
{
ROS_WARN("WmObjectCapture::updateObjectFrame %s",ex.what());
}
return true;
}
//ROS_INFO("TRANSFORM REJECTED ");
return false;
}
bool spawnObject(){
try{
ros::Time time = ros::Time::now();
tf_listener.waitForTransform("table_top", "object", time, ros::Duration(2.0));
tf_listener.lookupTransform("table_top", "object", time, transform);
}catch(tf::TransformException ex){
return false;
}
objectHeight = transform.getOrigin().getZ();
moveit_msgs::ApplyPlanningScene srv;
moveit_msgs::PlanningScene planning_scene;
planning_scene.is_diff = true;
moveit_msgs::CollisionObject object;
object.header.frame_id = "object";
object.id = "can";
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.CYLINDER;
primitive.dimensions.push_back(objectHeight); //height
primitive.dimensions.push_back(0.04); //radius
object.primitives.push_back(primitive);
geometry_msgs::Pose pose;
pose.orientation.w = 1.0;
pose.position.z = -objectHeight/2;
object.primitive_poses.push_back(pose);
place_pose.header.frame_id = "table_top";
place_pose.pose.orientation.x = 0.5;
place_pose.pose.orientation.y = 0.5;
place_pose.pose.orientation.z = -0.5;
place_pose.pose.orientation.w = 0.5;
place_pose.pose.position.x = transform.getOrigin().getX();
place_pose.pose.position.y = transform.getOrigin().getY();
place_pose.pose.position.z = transform.getOrigin().getZ() + 0.005;
object.operation = object.ADD;
planning_scene.world.collision_objects.push_back(object);
srv.request.scene = planning_scene;
planning_scene_diff_client.call(srv);
return true;
}
void InputOutputLinControl::getTrueRobotPose(double displacement, tf::StampedTransform robot_poseB, tf::StampedTransform& true_robot_pose)
{
true_robot_pose.frame_id_ = robot_poseB.frame_id_;
true_robot_pose.stamp_ = robot_poseB.stamp_;
double roll, pitch, yaw;
robot_poseB.getBasis().getRPY(roll,pitch,yaw);
tf::Vector3 v;
v.setX(robot_poseB.getOrigin().getX() - displacement*cos(yaw));
v.setY(robot_poseB.getOrigin().getY() - displacement*sin(yaw));
v.setZ(robot_poseB.getOrigin().getZ());
true_robot_pose.setOrigin(v);
true_robot_pose.setRotation(robot_poseB.getRotation());
}
void InputOutputLinControl::realRobotPoseB(double displacement, tf::StampedTransform real_robot_pose, tf::StampedTransform& real_robot_poseB)
{
real_robot_poseB.frame_id_ = real_robot_pose.frame_id_;
real_robot_poseB.stamp_ = real_robot_pose.stamp_;
real_robot_poseB.child_frame_id_ = real_robot_pose.child_frame_id_;
tf::Vector3 v;
double roll, pitch, yaw;
real_robot_pose.getBasis().getRPY(roll,pitch,yaw);
v.setX(real_robot_pose.getOrigin().getX() + displacement*cos(yaw));
v.setY(real_robot_pose.getOrigin().getY() + displacement*sin(yaw));
v.setZ(real_robot_pose.getOrigin().getZ());
real_robot_poseB.setOrigin(v);
real_robot_poseB.setRotation(real_robot_pose.getRotation());
}
geometry_msgs::Pose convertToPose(tf::StampedTransform transform)
{
tf::Quaternion q = transform.getRotation();
tf::Point p = transform.getOrigin();
geometry_msgs::Pose pose;
pose.position = createPoint(p.x(), p.y(), p.z());
pose.orientation = createQuaternion(q.x(), q.y(), q.z(), q.w());
return pose;
}
void StigmergyPlanner::makeViewFacingMarker(tf::StampedTransform robot_pose)
{
int_marker.header.frame_id = robot_pose.frame_id_;
int_marker.header.stamp = robot_pose.stamp_;
int_marker.pose.position.x = robot_pose.getOrigin().getX();
int_marker.pose.position.y = robot_pose.getOrigin().getY();
int_marker.pose.position.z = robot_pose.getOrigin().getZ();
int_marker.scale = 1;
int_marker.name = "view_facing";
int_marker.description = "3D Planning pencil";
InteractiveMarkerControl control;
// make a control that rotates around the view axis
//control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
//control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
//control.orientation.w = 1;
//control.name = "rotate";
//int_marker.controls.push_back(control);
// create a box in the center which should not be view facing,
// but move in the camera plane.
control.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
control.interaction_mode = InteractiveMarkerControl::MOVE_PLANE;
control.independent_marker_orientation = true;
control.name = "move";
control.markers.push_back( makeBox(int_marker) );
control.always_visible = true;
int_marker.controls.push_back(control);
control.interaction_mode = InteractiveMarkerControl::MENU;
control.name = "menu";
int_marker.controls.push_back(control);
server->insert(int_marker);
server->setCallback(int_marker.name, boost::bind(&StigmergyPlanner::processFeedback,this,_1));
menu_handler.apply(*server, int_marker.name);
}
void RockinPNPActionServer::tf2Affine(tf::StampedTransform& tf, Eigen::Affine3d& T)
{
tf::Vector3 o=tf.getOrigin();
tf::Quaternion q_tf=tf.getRotation();
Eigen::Quaterniond q(q_tf[3],q_tf[0],q_tf[1],q_tf[2]);
Eigen::Matrix3d R(q);
Eigen::Vector3d t(o[0],o[1],o[2]);
T.linear()=R; T.translation()=t;
}
void printTF(tf::StampedTransform t, WhichArm a)
{
std::string p;
if(a==RIGHT)
{
std::cout<< "#Right gripper:\n";
p="r";
}
else
{
std::cout<< "#Left gripper:\n";
p="l";
}
std::cout<< "p.position.x = " << t.getOrigin().getX() << ";\n"
<< "p.position.y = " << t.getOrigin().getY() << ";\n"
<< "p.position.z = " << t.getOrigin().getZ() << ";\n"
<< "p.orientation.x = " << t.getRotation().getX() << ";\n"
<< "p.orientation.y = " << t.getRotation().getY() << ";\n"
<< "p.orientation.z = " << t.getRotation().getZ() << ";\n"
<< "p.orientation.w = " << t.getRotation().getW() << ";\n";
geometry_msgs::PoseStamped tmp;
tmp.pose.position.x = t.getOrigin().getX() ;
tmp.pose.position.y = t.getOrigin().getY() ;
tmp.pose.position.z = t.getOrigin().getZ() ;
tmp.pose.orientation.x = t.getRotation().getX();
tmp.pose.orientation.y = t.getRotation().getY();
tmp.pose.orientation.z = t.getRotation().getZ();
tmp.pose.orientation.w = t.getRotation().getW();
std::cout<< " pose:\n"
<< " position:\n"
<< " x: " << tmp.pose.position.x << "\n"
<< " y: " << tmp.pose.position.y << "\n"
<< " z: " << tmp.pose.position.z << "\n"
<< " orientation:\n"
<< " x: " << tmp.pose.orientation.x << "\n"
<< " y: " << tmp.pose.orientation.y << "\n"
<< " z: " << tmp.pose.orientation.z << "\n"
<< " w: " << tmp.pose.orientation.w << "\n\n";
// std::cout<<tmp<<"\n";
}
void tfToPose(tf::StampedTransform &stampedTF, geometry_msgs::PoseStamped &msg)
{
tf::Vector3 translation = stampedTF.getOrigin();
msg.pose.position.x = translation.x();
msg.pose.position.y = translation.y();
msg.pose.position.z = translation.z();
tf::quaternionTFToMsg(stampedTF.getRotation(), msg.pose.orientation);
msg.header.stamp = stampedTF.stamp_;
msg.header.frame_id = stampedTF.frame_id_;
}
void tfToPose(tf::StampedTransform &stampedTF, geometry_msgs::Pose &msg)
{
tf::Vector3 translation = stampedTF.getOrigin();
msg.position.x = translation.x();
msg.position.y = translation.y();
msg.position.z = translation.z();
tf::quaternionTFToMsg(stampedTF.getRotation(), msg.orientation);
}
bool InputOutputLinControl::updateRobotPoseByModel(double timestep, double linear_vel, double angular_vel, tf::StampedTransform& pose)
{
pose.stamp_ = ros::Time::now();
double roll, pitch, yaw;
pose.getBasis().getRPY(roll,pitch,yaw);
tf::Vector3 v;
double theta = yaw + angular_vel * timestep;
//Eulerian integration
v.setX(pose.getOrigin().getX() + linear_vel * timestep * cos(theta));
v.setY(pose.getOrigin().getY() + linear_vel * timestep * sin(theta));
v.setZ(pose.getOrigin().getZ());
pose.setOrigin(v);
pose.setRotation(tf::createQuaternionFromYaw(theta));
ROS_INFO("New orientation [%f]",theta);
return true;
}
///////////////////////////////////////////////////////
/////-------Cordinate Convert function---------////////
///////////////////////////////////////////////////////
void pathLocal2Global(nav_msgs::Path& path, const tf::StampedTransform transform)
{
int length=path.poses.size();
nav_msgs::Odometry zero;
float angle = tf::getYaw(transform.getRotation()) - 0;
for(int i=0;i<length;i++){
float tmp_x = path.poses[i].pose.position.x - zero.pose.pose.position.x;
float tmp_y = path.poses[i].pose.position.y - zero.pose.pose.position.y;
float conv_x = cos(angle)*tmp_x - sin(angle)*tmp_y;
float conv_y = sin(angle)*tmp_x + cos(angle)*tmp_y;
path.poses[i].pose.position.x = conv_x + transform.getOrigin().x();
path.poses[i].pose.position.y = conv_y + transform.getOrigin().y();
}
}