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(); } }