void Hand_filter::update(tf::Vector3 p, tf::Quaternion& q){ if(b_first){ p_filter_buffer.push_back(p); q_filter_buffer.push_back(q); if(p_filter_buffer.size() == p_filter_buffer.capacity()){ b_first = false; ROS_INFO("====== hand filter ======"); // ROS_INFO("buffer full: %d",p_filter_buffer.size()); ROS_INFO("p: %f %f %f",p.x(),p.y(),p.z()); ROS_INFO("q: %f %f %f %f",q.x(),q.y(),q.z(),q.w()); k_position(0) = p.x();k_position(1) = p.y(); k_position(2) = p.z(); kalman_filter.Init(k_position); q_tmp = q; p_tmp = p; } }else{ /// Orientation filter if(jumped(q,q_tmp,q_threashold)){ ROS_INFO("q jumped !"); q = q_tmp; } q_attractor(q,up); q = q_tmp.slerp(q,0.1); /// Position filter if(!jumped(p,p_tmp,p_threashold)){ k_measurement(0) = p.x(); k_measurement(1) = p.y(); k_measurement(2) = p.z(); }else{ ROS_INFO("p jumped !"); k_measurement(0) = p_tmp.x(); k_measurement(1) = p_tmp.y(); k_measurement(2) = p_tmp.z(); } kalman_filter.Update(k_measurement,0.001); kalman_filter.GetPosition(k_position); p.setValue(k_position(0),k_position(1),k_position(2)); q_tmp = q; p_tmp = p; } }
Socket_one::Socket_one(std::string name, const tf::Vector3& origin, const tf::Vector3 &rpy,float scale){ //const std::string& name, const geo::fCVec3& C, const geo::fMat33& R, float r geo::fMat33 R; R.eye(); disk_radius = 0.02 * scale; hole_radius = 0.002 * scale; std::array<geo::Disk,3> holes; geo::fCVec3 position; position.zeros(); geo::Disk plate("plate",position,R,disk_radius); position.zeros(); position(0) = -0.01 * scale; holes[0] = geo::Disk("hole_left",position,R,hole_radius); position.zeros(); position(0) = 0.01 * scale; holes[1] = geo::Disk("hole_right",position,R,hole_radius); position.zeros(); position(1) = 0.005 * scale; holes[2] = geo::Disk("hole_up",position,R,hole_radius); wsocket = wobj::WSocket(name,plate,holes); geo::fCVec3 dim = {{0.07,0.07,0.05}}; dim = dim * scale; geo::fCVec3 orientation = {{0,0,0}}; position.zeros(); position(2) = -0.005/2 - (0.05 - 0.005)/2; wbox = wobj::WBox("box_socket_one",dim,position,orientation); // Transformation orientation(0) = rpy.x(); orientation(1) = rpy.y(); orientation(2) = rpy.z(); geo::fCVec3 T = {{origin.x(),origin.y(),origin.z()}}; wsocket.transform(T,orientation); wbox.transform(T,orientation); }
//Create a ROS frame out of the known corners of a tag in the weird marker coord frame used by Alvar markers (x right y forward z up) //p0-->p1 should point in Alvar's pos X direction //p1-->p2 should point in Alvar's pos Y direction int makeMasterTransform (const CvPoint3D64f& p0, const CvPoint3D64f& p1, const CvPoint3D64f& p2, const CvPoint3D64f& p3, tf::Transform &retT) { const tf::Vector3 q0(p0.x, p0.y, p0.z); const tf::Vector3 q1(p1.x, p1.y, p1.z); const tf::Vector3 q2(p2.x, p2.y, p2.z); const tf::Vector3 q3(p3.x, p3.y, p3.z); // (inverse) matrix with the given properties const tf::Vector3 v = (q1-q0).normalized(); const tf::Vector3 w = (q2-q1).normalized(); const tf::Vector3 n = v.cross(w); tf::Matrix3x3 m(v[0], v[1], v[2], w[0], w[1], w[2], n[0], n[1], n[2]); m = m.inverse(); //Translate to quaternion if(m.determinant() <= 0) return -1; //Use Eigen for this part instead, because the ROS version of bullet appears to have a bug Eigen::Matrix3f eig_m; for(int i=0; i<3; i++){ for(int j=0; j<3; j++){ eig_m(i,j) = m[i][j]; } } Eigen::Quaternion<float> eig_quat(eig_m); // Translate back to bullet tfScalar ex = eig_quat.x(); tfScalar ey = eig_quat.y(); tfScalar ez = eig_quat.z(); tfScalar ew = eig_quat.w(); tf::Quaternion quat(ex,ey,ez,ew); quat = quat.normalized(); double qx = (q0.x() + q1.x() + q2.x() + q3.x()) / 4.0; double qy = (q0.y() + q1.y() + q2.y() + q3.y()) / 4.0; double qz = (q0.z() + q1.z() + q2.z() + q3.z()) / 4.0; tf::Vector3 origin (qx,qy,qz); tf::Transform tform (quat, origin); //transform from master to marker retT = tform; return 0; }
geometry_msgs::Vector3 MoveBaseDoorAction::toVector(const tf::Vector3& pnt) { geometry_msgs::Vector3 res; res.x = pnt.x(); res.y = pnt.y(); res.z = pnt.z(); return res; }
/** * Convert tf::Vector3 to string */ template<> std::string toString<tf::Vector3>(const tf::Vector3& p_vec) { std::stringstream ss; ss << "(" << p_vec.x() << ", " << p_vec.y() << ", " << p_vec.z() << ")"; return ss.str(); }
gm::Point toPoint (const tf::Vector3& p) { gm::Point pt; pt.x = p.x(); pt.y = p.y(); pt.z = p.z(); return pt; }
inline void transformPointMatVec(const tf::Vector3 &origin, const tf::Matrix3x3 &basis, const geometry_msgs::Point32 &in, geometry_msgs::Point32 &out) { // Use temporary variables in case &in == &out double x = basis[0].x() * in.x + basis[0].y() * in.y + basis[0].z() * in.z + origin.x(); double y = basis[1].x() * in.x + basis[1].y() * in.y + basis[1].z() * in.z + origin.y(); double z = basis[2].x() * in.x + basis[2].y() * in.y + basis[2].z() * in.z + origin.z(); out.x = x; out.y = y; out.z = z; }
bool control(PID_control::controlserver::Request &req, PID_control::controlserver::Response &ret) { tf::vector3MsgToTF(req.target_error,sp); tf::transformMsgToTF(req.transform,pose); tf::vector3MsgToTF(req.velocity,vel); if (req.reset) { pParam=2,iParam=0,dParam=0,vParam=-2,cumul=0,lastE=0,pAlphaE=0,pBetaE=0,psp2=0,psp1=0,prevYaw=0; ROS_INFO("Controller reset"); } e = (pose*sp).z() - pose.getOrigin().z(); cumul=cumul+e; pv=pParam*e; thrust=5.335+pv+iParam*cumul+dParam*(e-lastE)+vel.z()*vParam; lastE=e; // Horizontal control: vx = pose*tf::Vector3(1,0,0); vy = pose*tf::Vector3(0,1,0); alphaE=(vy.z()-pose.getOrigin().z()); alphaCorr=0.25*alphaE+2.1*(alphaE-pAlphaE); betaE=(vx.z()-pose.getOrigin().z()); betaCorr=-0.25*betaE-2.1*(betaE-pBetaE); pAlphaE=alphaE; pBetaE=betaE; alphaCorr=alphaCorr+sp.y()*0.005+1*(sp.y()-psp2); betaCorr=betaCorr-sp.x()*0.005-1*(sp.x()-psp1); psp2=sp.y(); psp1=sp.x(); pose.getBasis().getRPY(t1, t2, yaw); rotCorr=yaw*0.1+2*(yaw-prevYaw); prevYaw = yaw; // Decide of the motor velocities: a=thrust*((double)1-alphaCorr+betaCorr+rotCorr); b=thrust*((double)1-alphaCorr-betaCorr-rotCorr); c=thrust*((double)1+alphaCorr-betaCorr+rotCorr); d=thrust*((double)1+alphaCorr+betaCorr-rotCorr); ret.a=a; ret.b=b; ret.c=c; ret.d=d; }
tf::Vector3 BasePositionPid::updateControl(const tf::Vector3& commanded_pos, const tf::Vector3& actual_pos, double time_elapsed) { double err_x = actual_pos.x() - commanded_pos.x() ; double err_y = actual_pos.y() - commanded_pos.y() ; double err_w = angles::shortest_angular_distance(commanded_pos.z(), actual_pos.z()) ; tf::Vector3 velocity_cmd ; double odom_cmd_x = pid_x_.updatePid(err_x, time_elapsed) ; // Translation X in the odometric frame double odom_cmd_y = pid_y_.updatePid(err_y, time_elapsed) ; // Translation Y in the odometric frame // Rotate the translation commands so that they're in the base frame (instead of the odom frame) velocity_cmd.setX( odom_cmd_x*cos(actual_pos.z()) + odom_cmd_y*sin(actual_pos.z())) ; velocity_cmd.setY(-odom_cmd_x*sin(actual_pos.z()) + odom_cmd_y*cos(actual_pos.z())) ; velocity_cmd.setZ(pid_w_.updatePid(err_w, time_elapsed)) ; // Rotation command is same is Odom and Base frames return velocity_cmd ; }
static carmen_vector_3D_t tf_vector3_to_carmen_vector3(tf::Vector3 tf_vector) { carmen_vector_3D_t carmen_vector; carmen_vector.x = tf_vector.x(); carmen_vector.y = tf_vector.y(); carmen_vector.z = tf_vector.z(); return carmen_vector; }
std::vector<geometry_msgs::Point> MoveBaseDoorAction::getOrientedFootprint(const tf::Vector3 pos, double theta_cost) { double cos_th = cos(theta_cost); double sin_th = sin(theta_cost); std::vector<geometry_msgs::Point> oriented_footprint; for(unsigned int i = 0; i < footprint_.size(); ++i){ geometry_msgs::Point new_pt; new_pt.x = pos.x() + (footprint_[i].x * cos_th - footprint_[i].y * sin_th); new_pt.y = pos.y() + (footprint_[i].x * sin_th + footprint_[i].y * cos_th); oriented_footprint.push_back(new_pt); } return oriented_footprint; }
void MetalDetectorCallback(const std_msgs::Float32 v) { static int sMarkerID=0; if(v.data>0.95) // Mine detected { //ROS_INFO("Found a mine"); // Check whether a mine at the same position has already been found for(auto it = m_vMinesPositions.begin(); it!= m_vMinesPositions.end(); ++it) { if(squaredDistance2D(it->x(), it->y(), m_WorldSpaceToolPosition.x(), m_WorldSpaceToolPosition.y())<0.04) return; } //ROS_INFO("Publishing marker"); visualization_msgs::Marker m; m.header.stamp = m_MsgHeaderStamp; m.header.frame_id = "/world"; m.ns = "mine"; m.id = sMarkerID++; m_vMinesPositions.push_back(m_WorldSpaceToolPosition); m.type = visualization_msgs::Marker::CYLINDER; m.action = visualization_msgs::Marker::ADD; m.pose.position.x = m_WorldSpaceToolPosition.x(); m.pose.position.y = m_WorldSpaceToolPosition.y(); m.pose.position.z = m_WorldSpaceToolPosition.z(); m.scale.x = 0.4; m.scale.y = 0.4; m.scale.z = 0.4; m.color.a = 0.5; m.color.r = 0.0; m.color.g = 1.0; m.color.b = 0.0; //m.frame_locked = true; // Finally publish the marker m_MineMarkerPublisher.publish(m); } }
void CameraTransformOptimization::calculateMarkerError(CalibrationState state, tf::Vector3 markerPoint, float& error) { error = 0; for (int i = 0; i < this->measurePoints.size(); i++) { float currentError = 0; MeasurePoint& current = this->measurePoints[i]; tf::Transform opticalToFixed = current.opticalToFixed(state); tf::Vector3 transformedPoint = opticalToFixed * current.measuredPosition; currentError += pow(markerPoint.x() - transformedPoint.x(), 2); currentError += pow(markerPoint.y() - transformedPoint.y(), 2); currentError += pow(markerPoint.z() - transformedPoint.z(), 2); error += currentError; } }
void MTMHaptics::convert_bodyForcetoSpatialForce(geometry_msgs::WrenchStamped &body_wrench){ visualize_haptic_force(body_force_pub); rot_quat.setX(cur_mtm_pose.orientation.x); rot_quat.setY(cur_mtm_pose.orientation.y); rot_quat.setZ(cur_mtm_pose.orientation.z); rot_quat.setW(cur_mtm_pose.orientation.w); F7wrt0.setValue(body_wrench.wrench.force.x, body_wrench.wrench.force.y, body_wrench.wrench.force.z); rot_matrix.setRotation(rot_quat); F0wrt7 = rot_matrix.transpose() * F7wrt0; body_wrench.wrench.force.x = F0wrt7.x(); body_wrench.wrench.force.y = F0wrt7.y(); body_wrench.wrench.force.z = F0wrt7.z(); visualize_haptic_force(spatial_force_pub); }
void Jumps::update(tf::Vector3& origin,tf::Quaternion& orientation){ if(bFirst){ origin_buffer.push_back(origin); orientation_buffer.push_back(orientation); if(origin_buffer.size() == origin_buffer.capacity()){ bFirst = false; ROS_INFO("====== jump filter full ======"); } }else{ origin_tmp = origin_buffer[origin_buffer.size()-1]; orientation_tmp = orientation_buffer[orientation_buffer.size()-1]; if(bDebug){ std::cout<< "=== jum debug === " << std::endl; std::cout<< "p : " << origin.x() << "\t" << origin.y() << "\t" << origin.z() << std::endl; std::cout<< "p_tmp: " << origin_tmp.x() << "\t" << origin_tmp.y() << "\t" << origin_tmp.z() << std::endl; std::cout<< "p_dis: " << origin.distance(origin_tmp) << std::endl; std::cout<< "q : " << orientation.x() << "\t" << orientation.y() << "\t" << orientation.z() << "\t" << orientation.w() << std::endl; std::cout<< "q_tmp: " << orientation_tmp.x() << "\t" << orientation_tmp.y() << "\t" << orientation_tmp.z() << "\t" << orientation_tmp.w() << std::endl; std::cout<< "q_dis: " << dist(orientation,orientation_tmp) << std::endl; } /// Position jump if(jumped(origin,origin_tmp,origin_threashold)){ ROS_INFO("position jumped !"); origin = origin_tmp; // exit(0); }else{ origin_buffer.push_back(origin); } /// Orientation jump if(jumped(orientation,orientation_tmp,orientation_threashold)){ ROS_INFO("orientation jumped !"); orientation = orientation_tmp; //exit(0); }else{ orientation_buffer.push_back(orientation); } } }
float JPCT_Math::calcAngle(tf::Vector3 v1, tf::Vector3 v2) { float dot = v1.x() * v2.x() + v1.y() * v2.y() + v1.z() * v2.z(); float lt = v1.x() * v1.x() + v1.y() * v1.y() + v1.z() * v1.z(); float lv = v2.x() * v2.x() + v2.y() * v2.y() + v2.z() * v2.z(); dot /= sqrt(lt * lv); if(dot < -1.0f) { dot = -1.0f; } else if(dot > 1.0f) { dot = 1.0f; } return acos(dot); }
tf::Matrix3x3 JPCT_Math::rotateAxis(tf::Vector3 axis, tf::Matrix3x3& original, float angle) { if(angle != lastRot) { lastRot = angle; lastSin = (float)sin((double)angle); lastCos = (float)cos((double)angle); } float c = lastCos; float s = lastSin; float t = 1.0F - c; //axis = axis.normalize(axis); float x = axis.x(); float y = axis.y(); float z = axis.z(); tf::Matrix3x3 mat; mat.setIdentity(); float sy = s * y; float sx = s * x; float sz = s * z; float txy = t * x * y; float txz = t * x * z; float tyz = t * y * z; mat[0][0] = t * x * x + c; mat[1][0] = txy + sz; mat[2][0] = txz - sy; mat[0][1] = txy - sz; mat[1][1] = t * y * y + c; mat[2][1] = tyz + sx; mat[0][2] = txz + sy; mat[1][2] = tyz - sx; mat[2][2] = t * z * z + c; orthonormalize(mat); original *= mat; return original; }
/** * @brief Creates a sphere RViz marker for publishing * @param color_g The color for the marker, in range of 0 to 1, 0 is pure red, 1 is pure green * @param sphere_size The radius of the sphere * @param pos The position of the marker on the map * @return A marker */ visualization_msgs::Marker prepareSphereMarker(double color_g, double sphere_size, tf::Vector3& pos) { visualization_msgs::Marker marker; marker.header.frame_id = "/map"; marker.header.stamp = ros::Time::now(); marker.ns = "wifi_heatmap"; marker.id = marker_id++; marker.type = visualization_msgs::Marker::SPHERE; // Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) 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 marker.pose.position.x = pos.x(); marker.pose.position.y = pos.y(); marker.pose.position.z = pos.z(); marker.pose.orientation.x = 0.0; marker.pose.orientation.y = 0.0; marker.pose.orientation.z = 0.0; marker.pose.orientation.w = 1.0; // Set the scale of the marker marker.scale.x = sphere_size; marker.scale.y = sphere_size; marker.scale.z = sphere_size; // Set the color -- be sure to set alpha to something non-zero! marker.color.r = 1.0 - color_g; marker.color.g = color_g; marker.color.b = 0.0f; marker.color.a = 1.0; marker.lifetime = ros::Duration(); return marker; }
void gotIntersection(const tf::Vector3 &pt) { sendPoint(pt.x(), pt.y(), pt.z()); }
void Robot::move(tf::Vector3 speed){ geometry_msgs::Twist msg; msg.linear.x = speed.x(); msg.linear.y = speed.y(); speeds_publisher.publish(msg); }
void make_markers2(tf::Vector3 point1, tf::Vector3 point2, tf::Vector3 point3, tf::Vector3 origin) { visualization_msgs::Marker marker1a; marker1a.header.frame_id = world_frame_; marker1a.header.stamp = ros::Time(); marker1a.id = 10; marker1a.type = visualization_msgs::Marker::SPHERE; marker1a.action = visualization_msgs::Marker::ADD; marker1a.pose.position.x = point1.x(); marker1a.pose.position.y = point1.y(); marker1a.pose.position.z = point1.z(); marker1a.color.a = 1.0; marker1a.color.r = 0.0; marker1a.color.g = 1.0; marker1a.color.b = 0.0; marker1a.scale.x = 0.04; marker1a.scale.y = 0.04; marker1a.scale.z = 0.04; markers_.markers.push_back(marker1a); visualization_msgs::Marker marker2a; marker2a.header.frame_id = world_frame_; marker2a.header.stamp = ros::Time(); marker2a.id = 11; marker2a.type = visualization_msgs::Marker::SPHERE; marker2a.action = visualization_msgs::Marker::ADD; marker2a.pose.position.x = point2.x(); marker2a.pose.position.y = point2.y(); marker2a.pose.position.z = point2.z(); marker2a.color.a = 1.0; marker2a.color.r = 1.0; marker2a.color.g = 0.0; marker2a.color.b = 0.0; marker2a.scale.x = 0.04; marker2a.scale.y = 0.04; marker2a.scale.z = 0.04; markers_.markers.push_back(marker2a); visualization_msgs::Marker marker3a; marker3a.header.frame_id = world_frame_; marker3a.header.stamp = ros::Time(); marker3a.id = 12; marker3a.type = visualization_msgs::Marker::SPHERE; marker3a.action = visualization_msgs::Marker::ADD; marker3a.pose.position.x = point3.x(); marker3a.pose.position.y = point3.y(); marker3a.pose.position.z = point3.z(); marker3a.color.a = 1.0; marker3a.color.r = 0.0; marker3a.color.g = 0.0; marker3a.color.b = 1.0; marker3a.scale.x = 0.04; marker3a.scale.y = 0.04; marker3a.scale.z = 0.04; markers_.markers.push_back(marker3a); visualization_msgs::Marker marker2; marker2.header.frame_id = world_frame_; marker2.header.stamp = ros::Time(); marker2.id = 13; marker2.type = visualization_msgs::Marker::SPHERE; marker2.action = visualization_msgs::Marker::ADD; marker2.pose.position.x = origin.x(); marker2.pose.position.y = origin.y(); marker2.pose.position.z = origin.z(); marker2.color.a = 1.0; marker2.color.r = 0.5; marker2.color.g = 0.0; marker2.color.b = 0.5; marker2.scale.x = 0.02; marker2.scale.y = 0.02; marker2.scale.z = 0.02; markers_.markers.push_back(marker2); //vis_pub.publish(markers_); }
void leatherman::tfVector3ToEigen(const tf::Vector3 &bt, Eigen::Vector3d &e) { e(0) = bt.x(); e(1) = bt.y(); e(2) = bt.z(); }
void aero_path_planning::vectorToPoint(const tf::Vector3& vector, aero_path_planning::Point& point) { point.x = vector.x(); point.y = vector.y(); point.z = vector.z(); }