Esempio n. 1
0
const float Utility::findAngleFromAToB(const tf::Vector3 a, const tf::Vector3 b) const {
  std::vector<float> a_vec;
  a_vec.push_back(a.getX());
  a_vec.push_back(a.getY());

  std::vector<float> b_vec;
  b_vec.push_back(b.getX());
  b_vec.push_back(b.getY());

  return findAngleFromAToB(a_vec, b_vec);
}
Esempio n. 2
0
    float getVel(tf::Vector3 & pose1, tf::Vector3 & pose2, const ros::Duration & dur)
    {
      float vel;
      float delta_x = fabs(pose1.getX() - pose2.getX());
      float delta_y = fabs(pose1.getY() - pose2.getY());
      float dist = sqrt(delta_x*delta_x + delta_y*delta_y);

      vel = dist / dur.toSec();

      return vel;
    }
Esempio n. 3
0
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;
}
Esempio n. 4
0
tf::Vector3 JPCT_Math::rotate(tf::Vector3 vec, tf::Matrix3x3& mat) {
    float xr = vec.getX() * mat[0][0] + vec.getY() * mat[1][0] + vec.getZ() * mat[2][0];
    float yr = vec.getX() * mat[0][1] + vec.getY() * mat[1][1] + vec.getZ() * mat[2][1];
    float zr = vec.getX() * mat[0][2] + vec.getY() * mat[1][2] + vec.getZ() * mat[2][2];
    vec.setX(xr);
    vec.setY(yr);
    vec.setZ(zr);

    return vec;
}              
Esempio n. 5
0
double getMinimumDistance(tf::Vector3 position, nav_msgs::GridCells grid) {

  double distance_sq=-1;
  int size = grid.cells.size();
  double my_x=position.getX(), my_y=position.getY();
  double stop_radius_sq = stop_radius*stop_radius;
  for (int i=0; i<size; i++) {
    double d_sq = pow(grid.cells[i].x-my_x,2)+pow(grid.cells[i].y-my_y,2);
    if (distance_sq==-1 || d_sq<distance_sq) distance_sq=d_sq;
    if (d_sq<=stop_radius_sq) return 0;
  }

  return sqrt(distance_sq);
}
Esempio n. 6
0
 double DistanceFromPlane(
     const tf::Vector3& plane_normal,
     const tf::Vector3& plane_point,
     const tf::Vector3& point)
 {
   return plane_normal.normalized().dot(point - plane_point);
 }
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;
	}
}
Esempio n. 8
0
 double DistanceFromLineSegment(
     const tf::Vector3& line_start,
     const tf::Vector3& line_end,
     const tf::Vector3& point)
 {    
   return point.distance(ProjectToLineSegment(line_start, line_end, point));
 }
Esempio n. 9
0
bool Hand_filter::jumped(const tf::Vector3& p_current,const tf::Vector3& p_previous, const float threashold) const{
    if(p_current.distance(p_previous) < threashold){
        return false;
    }else{
        return true;
    }
}
Esempio n. 10
0
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);

}
Esempio n. 11
0
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);
        }
    }
}
Esempio n. 12
0
tf::Matrix3x3 GetTfRotationMatrix(tf::Vector3 xaxis, tf::Vector3 zaxis) {
    tf::Matrix3x3 m;
    tf::Vector3 yaxis = zaxis.cross(xaxis);
    m.setValue(xaxis.getX(), yaxis.getX(), zaxis.getX(),
            xaxis.getY(), yaxis.getY(), zaxis.getY(),
            xaxis.getZ(), yaxis.getZ(), zaxis.getZ());
    return m;
}
Esempio n. 13
0
bool HapticsPSM::has_normal_changed(tf::Vector3 &v1, tf::Vector3 &v2){
    tfScalar angle;
    angle = v1.angle(v2);
    if(angle > coll_psm.epsilon){
        return true; //The new normal has changed
    }
    else{
        return false; //The new normal has not changed
    }
}
/* Dynamic reconfigure callback */
void BeaconKFNode::configCallback(beacon_localizer::beacon_localizer_paramsConfig &config, uint32_t level)
{
    double platform_angle;
    int platform_index;
    
    _reference_coords[0] = config.reference_x;
    _reference_coords[1] = config.reference_y;

    //get platform orientation in degrees, convert to reference system
    platform_angle =  config.platform_orientation;
    platform_angle = platform_angle*(M_PI/180);
    platform_angle = platform_angle + atan(_reference_coords[1]/_reference_coords[0]);
    _platform_orientation = tf::createQuaternionFromYaw(platform_angle);

    platform_index = config.platform_index;
    _platform_origin.setX(_platform_x_coords[platform_index]);
    _platform_origin.setY(_platform_y_coords[platform_index]);
    
}
Esempio n. 15
0
int extractFrame (const pcl::ModelCoefficients& coeffs,
                  const ARPoint& p1, const ARPoint& p2,
                  const ARPoint& p3, const ARPoint& p4,
                  tf::Matrix3x3 &retmat)
{
    // Get plane coeffs and project points onto the plane
    double a=0, b=0, c=0, d=0;
    if(getCoeffs(coeffs, &a, &b, &c, &d) < 0)
        return -1;

    const tf::Vector3 q1 = project(p1, a, b, c, d);
    const tf::Vector3 q2 = project(p2, a, b, c, d);
    const tf::Vector3 q3 = project(p3, a, b, c, d);
    const tf::Vector3 q4 = project(p4, a, b, c, d);

    // Make sure points aren't the same so things are well-defined
    if((q2-q1).length() < 1e-3)
        return -1;

    // (inverse) matrix with the given properties
    const tf::Vector3 v = (q2-q1).normalized();
    const tf::Vector3 n(a, b, c);
    const tf::Vector3 w = -v.cross(n);
    tf::Matrix3x3 m(v[0], v[1], v[2], w[0], w[1], w[2], n[0], n[1], n[2]);

    // Possibly flip things based on third point
    const tf::Vector3 diff = (q4-q3).normalized();
    //ROS_INFO_STREAM("w = " << w << " and d = " << diff);
    if (w.dot(diff)<0)
    {
        //ROS_INFO_STREAM("Flipping normal based on p3.  Current value: " << m);
        m[1] = -m[1];
        m[2] = -m[2];
        //ROS_INFO_STREAM("New value: " << m);
    }

    // Invert and return
    retmat = m.inverse();
    //cerr << "Frame is " << retmat << endl;
    return 0;
}
Esempio n. 16
0
		/********** callback for the cmd velocity from the autonomy **********/
		void cmd_vel_callback(const geometry_msgs::Twist& msg)
		{
			watchdogTimer.stop();
			
			error.setValue(msg.linear.x - body_vel.linear.x, msg.linear.y - body_vel.linear.y, msg.linear.z - body_vel.linear.z);
			//std::cout << "error x: " << error.getX() << " y: " << error.getY() << " z: " << error.getZ() << std::endl;
			//std::cout << std::abs(curr_body_vel_time.toSec() - last_body_vel_time.toSec()) << std::endl;
			error_yaw = msg.angular.z - body_vel.angular.z;
			//std::cout << "error yaw: " << error_yaw << std::endl;
			
			// if some time has passed between the last body velocity time and the current body velocity time then will calculate the (feed forward PD)
			if (std::abs(curr_body_vel_time.toSec() - last_body_vel_time.toSec()) > 0.00001)
			{	
				errorDot = (1/(curr_body_vel_time - last_body_vel_time).toSec()) * (error - last_error);
				//std::cout << "errordot x: " << errorDot.getX() << " y: " << errorDot.getY() << " z: " << errorDot.getZ() << std::endl;
				
				errorDot_yaw = (1/(curr_body_vel_time - last_body_vel_time).toSec()) * (error_yaw - last_error_yaw);
				//std::cout << "error dot yaw " << errorDot_yaw << std::endl;
				velocity_command.linear.x = cap_vel_auton(kx*msg.linear.x + (kp*error).getX() + (kd*errorDot).getX());
				velocity_command.linear.y = cap_vel_auton(ky*msg.linear.y + (kp*error).getY() + (kd*errorDot).getY());
				velocity_command.linear.z = cap_vel_auton(kz*msg.linear.z + (kp*error).getZ() + (kd*errorDot).getZ());
				velocity_command.angular.z = -1*cap_vel_auton(kyaw*msg.angular.z + kp_yaw*error_yaw + kd_yaw*errorDot_yaw); // z must be switched because bebop driver http://bebop-autonomy.readthedocs.org/en/latest/piloting.html
			}
			
			last_body_vel_time = curr_body_vel_time;// update last time body velocity was recieved
			last_error = error;
			last_error_yaw = error_yaw;
			
			error_gm.linear.x = error.getX(); error_gm.linear.y = error.getY(); error_gm.linear.z = error.getZ(); error_gm.angular.z = error_yaw;
			errorDot_gm.linear.x = errorDot.getX(); errorDot_gm.linear.y = errorDot.getY(); errorDot_gm.linear.z = errorDot.getZ(); errorDot_gm.angular.z = kyaw*msg.angular.z + kp_yaw*error_yaw + kd_yaw*errorDot_yaw;
			error_pub.publish(error_gm);
			errorDot_pub.publish(errorDot_gm);
			
			if (start_autonomous)
			{
				recieved_command_from_tracking = true;
			}
			
			watchdogTimer.start();
		}
Esempio n. 17
0
GridRayTrace::GridRayTrace(tf::Vector3 start, tf::Vector3 end, const OccupancyGrid& grid_ref)
	: _x_store(), _y_store()
{
	//Transform (x,y) into map frame
	start = grid_ref.toGridFrame(start);
	end = grid_ref.toGridFrame(end);
	
	double x0 = start.getX(), y0 = start.getY();
	double x1 = end.getX(), y1 = end.getY();
	
	//ROS_WARN("start: (%f, %f) -> end: (%f, %f)", x0, y0, x1, y1);
	
	bresenham
		(
			floor(x0), floor(y0), 
			floor(x1), floor(y1), 
			_x_store, _y_store
		);
	
	_cur_idx = 0;
	_max_idx = std::min(_x_store.size(), _y_store.size());
}
bool planning_environment::configureForAttachedBodyMask(planning_models::KinematicState& state,
        planning_environment::CollisionModels* cm,
        tf::TransformListener& tf,
        const std::string& sensor_frame,
        const ros::Time& sensor_time,
        tf::Vector3& sensor_pos)
{
    state.setKinematicStateToDefault();

    cm->bodiesLock();

    const std::map<std::string, std::map<std::string, bodies::BodyVector*> >& link_att_objects = cm->getLinkAttachedObjects();

    if(link_att_objects.empty()) {
        cm->bodiesUnlock();
        return false;
    }

    planning_environment::updateAttachedObjectBodyPoses(cm,
            state,
            tf);

    sensor_pos.setValue(0.0,0.0,0.0);

    // compute the origin of the sensor in the frame of the cloud
    if (!sensor_frame.empty()) {
        std::string err;
        try {
            tf::StampedTransform transf;
            tf.lookupTransform(cm->getWorldFrameId(), sensor_frame, sensor_time, transf);
            sensor_pos = transf.getOrigin();
        } catch(tf::TransformException& ex) {
            ROS_ERROR("Unable to lookup transform from %s to %s. Exception: %s", sensor_frame.c_str(), cm->getWorldFrameId().c_str(), ex.what());
            sensor_pos.setValue(0, 0, 0);
        }
    }
    cm->bodiesUnlock();
    return true;
}
Esempio n. 19
0
	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);
		}
	}
Esempio n. 20
0
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; 
}
Esempio n. 21
0
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;
}
Esempio n. 22
0
  /**
  * @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;
  }
int farthestPoint(const tf::Vector3& point, const std::vector<tf::Vector3>& hand_positions)
{
  double dist, max_distant = 0;
  size_t index = 0;
  for (size_t i = 0; i < hand_positions.size(); i++)
  {
    dist = point.distance(hand_positions[i]);
    if (dist > max_distant)
    {
      max_distant = dist;
      index = i;
    }
  }
  return index;
}
Esempio n. 24
0
void Hand_filter::align_quaternion_axis(tf::Quaternion& q_out, float &angle, const tf::Quaternion &q_in, tf::Vector3 target){
    tf::Matrix3x3 R;  R.setRotation(q_in);
    tf::Vector3 z_axis(R[0][2],R[1][2],R[2][2]);

    z_axis          = z_axis.normalize();

    tf::Vector3 c   = z_axis.cross(target);
    angle           = z_axis.dot(target);

    tf::Quaternion q_r;
    q_r.setX(c.x());
    q_r.setY(c.y());
    q_r.setZ(c.z());
    q_r.setW(sqrt(z_axis.length2() * target.length2()) + angle);

    q_out = q_r*q_in;
}
Esempio n. 25
0
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 ;
}
Esempio n. 26
0
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);

}
Esempio n. 27
0
Point calcSquareNormGrad(tf::Vector3 bl,tf::Vector3 br,tf::Vector3 tr,tf::Vector3 tl){
  //ROS_INFO("[calcSquareNormGrad]\tentering...");

    double dx = ((bl.getZ() - br.getZ()) + (tl.getZ() - tr.getZ()))/2.0;
    double dy = ((tl.getZ() - bl.getZ()) + (tr.getZ() - br.getZ()))/2.0;
    double total_dist = pow( pow(dx, 2) + pow(dy,2) , 0.5);

    if(total_dist != 0){
        Point p = Point(0.5*dx/total_dist, 0.5*dy/total_dist);
        return p;
    }

    //if were on a plateau, just move a small distance towards the goal
    double xd = (g_pos.x - r_pos.x);
    double yd = (g_pos.y - r_pos.y);
    if(abs(xd) > 1.0) xd = xd/(abs(xd));
    if(abs(yd) > 1.0) yd = yd/(abs(yd));
    Point toreturn = Point(xd, yd);
    return toreturn;
}
Esempio n. 28
0
double getSafeVelocityLimit(tf::Vector3 vel) {
  //get current position + orientation
  //   by listening to the transform from obstacles->header->frame_id to /base_link
  tf::StampedTransform transform;
  try {
    tf_listener->lookupTransform(
        obstacles.header.frame_id,
        "/base_link",
        ros::Time(0),
        transform);
  }
  catch (tf::TransformException ex) {
    ROS_ERROR("Couldn't get current position: %s",ex.what());
    return min_spd;
  }
  //estimate future position, current_position+cmd_vel*sometimeconst*orientation
  tf::Vector3 future_pos = transform.getOrigin();
  tf::Vector3 v = vel.rotate(tf::Vector3(0,0,1),tf::getYaw(transform.getRotation()));
  future_pos=future_pos+timestep*v;
  //get minimum distance of future position
  if (!has_grid) {
    ROS_ERROR("Haven't received a grid yet so speed will be fully limited");
    return min_spd;
  }
  double mindist = getMinimumDistance(future_pos,obstacles);
  ROS_DEBUG("got minimum distance %f",mindist);
  //calculate velocity scale based on mindist of futurepos 
  //   (minspd@<=minrad --lerp--> 1@>=maxrad)
  double speed_limit;
  if (mindist>inflation_radius) speed_limit=max_spd;
  else if (mindist<=stop_radius) speed_limit=min_spd;
  else speed_limit = min(min_spd+(max_spd-min_spd)*(mindist-stop_radius)/(inflation_radius-stop_radius),max_spd);
  //return it
  ROS_DEBUG_STREAM("  computed velocity scale: "<<speed_limit);
  return speed_limit;
}
bool checkCloud(const sensor_msgs::PointCloud2& cloud_msg,
                typename pcl::PointCloud<T>::Ptr hand_cloud,
                //typename pcl::PointCloud<T>::Ptr finger_cloud,
                const std::string& frame_id,
                tf::Vector3& hand_position,
                tf::Vector3& arm_position,
                tf::Vector3& arm_direction)
{  
  typename pcl::PointCloud<T>::Ptr cloud(new pcl::PointCloud<T>);
  pcl::fromROSMsg(cloud_msg, *cloud);

  if((cloud->points.size() < g_config.min_cluster_size) ||
     (cloud->points.size() > g_config.max_cluster_size))
    return false;

  pcl::PCA<T> pca;
  pca.setInputCloud(cloud);
  Eigen::Vector4f mean = pca.getMean();

  if((mean.coeff(0) < g_config.min_x) || (mean.coeff(0) > g_config.max_x)) return false;
  if((mean.coeff(1) < g_config.min_y) || (mean.coeff(1) > g_config.max_y)) return false;
  if((mean.coeff(2) < g_config.min_z) || (mean.coeff(2) > g_config.max_z)) return false;

  Eigen::Vector3f eigen_value = pca.getEigenValues();
  double ratio = eigen_value.coeff(0) / eigen_value.coeff(1);

  if((ratio < g_config.min_eigen_value_ratio) || (ratio > g_config.max_eigen_value_ratio)) return false;

  T search_point;
  Eigen::Matrix3f ev = pca.getEigenVectors();
  Eigen::Vector3f main_axis(ev.coeff(0, 0), ev.coeff(1, 0), ev.coeff(2, 0));
  main_axis.normalize();
  arm_direction.setX(main_axis.coeff(0));
  arm_direction.setY(main_axis.coeff(1));
  arm_direction.setZ(main_axis.coeff(2));
  arm_position.setX(mean.coeff(0));
  arm_position.setY(mean.coeff(1));
  arm_position.setZ(mean.coeff(2));

  main_axis = (-main_axis * 0.3) + Eigen::Vector3f(mean.coeff(0), mean.coeff(1), mean.coeff(2));
  search_point.x = main_axis.coeff(0);
  search_point.y = main_axis.coeff(1);
  search_point.z = main_axis.coeff(2);



  //find hand
  pcl::KdTreeFLANN<T> kdtree;
  kdtree.setInputCloud(cloud);

  //find the closet point from the serach_point
  std::vector<int> point_indeices(1);
  std::vector<float> point_distances(1);
  if ( kdtree.nearestKSearch (search_point, 1, point_indeices, point_distances) > 0 )
  {
    //update search point
    search_point = cloud->points[point_indeices[0]];

    //show seach point
    if(g_marker_array_pub.getNumSubscribers() != 0)
    {
      pushSimpleMarker(search_point.x, search_point.y, search_point.z,
                       1.0, 0, 0,
                       0.02,
                       g_marker_id, g_marker_array, frame_id);
    }

    //hand
    point_indeices.clear();
    point_distances.clear();
    kdtree.radiusSearch(search_point, g_config.hand_lenght, point_indeices, point_distances);
    for (size_t i = 0; i < point_indeices.size (); ++i)
    {
      hand_cloud->points.push_back(cloud->points[point_indeices[i]]);
      hand_cloud->points.back().r = 255;
      hand_cloud->points.back().g = 0;
      hand_cloud->points.back().b = 0;
    }

    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(*hand_cloud, centroid);

    if(g_marker_array_pub.getNumSubscribers() != 0)
    {
      pushSimpleMarker(centroid.coeff(0), centroid.coeff(1), centroid.coeff(2),
                       0.0, 1.0, 0,
                       0.02,
                       g_marker_id, g_marker_array, frame_id);
    }

    hand_position.setX(centroid.coeff(0));
    hand_position.setY(centroid.coeff(1));
    hand_position.setZ(centroid.coeff(2));

#if 0
    //fingers
    search_point.x = centroid.coeff(0);
    search_point.y = centroid.coeff(1);
    search_point.z = centroid.coeff(2);
    std::vector<int> point_indeices_inner;
    std::vector<float> point_distances_inner;
    kdtree.radiusSearch(search_point, 0.07, point_indeices_inner, point_distances_inner);

    std::vector<int> point_indeices_outter;
    std::vector<float> point_distances_outter;
    kdtree.radiusSearch(search_point, 0.1, point_indeices_outter, point_distances_outter);

    //ROS_INFO("before %d %d", point_indeices_inner.size(), point_indeices_outter.size());

    std::vector<int>::iterator it;
    for(size_t i = 0; i < point_indeices_inner.size(); i++)
    {

      it =  std::find(point_indeices_outter.begin(), point_indeices_outter.end(), point_indeices_inner[i]);
      if(it != point_indeices_outter.end())
      {
        point_indeices_outter.erase(it);
      }
    }

    //ROS_INFO("after %d %d", point_indeices_inner.size(), point_indeices_outter.size());

    //ROS_DEBUG_THROTTLE(1.0, "found %lu", point_indeices.size ());
    for (size_t i = 0; i < point_indeices_outter.size (); ++i)
    {
      finger_cloud->points.push_back(cloud->points[point_indeices_outter[i]]);
      finger_cloud->points.back().r = 255;
      finger_cloud->points.back().g = 0;
      finger_cloud->points.back().b = 0;
    }
#endif

  }
  else
  {    
    return false;
  }

  if(g_marker_array_pub.getNumSubscribers() != 0)
  {
    pushEigenMarker<T>(pca, g_marker_id, g_marker_array, 0.1, frame_id);
  }


  return true;
}
bool
ArucoMapping::processImage(cv::Mat input_image,cv::Mat output_image)
{
  aruco::MarkerDetector Detector;
  std::vector<aruco::Marker> temp_markers;

  //Set visibility flag to false for all markers
  for(size_t i = 0; i < num_of_markers_; i++)
      markers_[i].visible = false;

  // Save previous marker count
  marker_counter_previous_ = marker_counter_;

  // Detect markers
  Detector.detect(input_image,temp_markers,aruco_calib_params_,marker_size_);
    
  // If no marker found, print statement
  if(temp_markers.size() == 0)
    ROS_DEBUG("No marker found!");

  //------------------------------------------------------
  // FIRST MARKER DETECTED
  //------------------------------------------------------
  if((temp_markers.size() > 0) && (first_marker_detected_ == false))
  {
    //Set flag
    first_marker_detected_=true;

    // Detect lowest marker ID
    lowest_marker_id_ = temp_markers[0].id;
    for(size_t i = 0; i < temp_markers.size();i++)
    {
      if(temp_markers[i].id < lowest_marker_id_)
        lowest_marker_id_ = temp_markers[i].id;
    }


    ROS_DEBUG_STREAM("The lowest Id marker " << lowest_marker_id_ );

    // Identify lowest marker ID with world's origin
    markers_[0].marker_id = lowest_marker_id_;

    markers_[0].geometry_msg_to_world.position.x = 0;
    markers_[0].geometry_msg_to_world.position.y = 0;
    markers_[0].geometry_msg_to_world.position.z = 0;

    markers_[0].geometry_msg_to_world.orientation.x = 0;
    markers_[0].geometry_msg_to_world.orientation.y = 0;
    markers_[0].geometry_msg_to_world.orientation.z = 0;
    markers_[0].geometry_msg_to_world.orientation.w = 1;

    // Relative position and Global position
    markers_[0].geometry_msg_to_previous.position.x = 0;
    markers_[0].geometry_msg_to_previous.position.y = 0;
    markers_[0].geometry_msg_to_previous.position.z = 0;

    markers_[0].geometry_msg_to_previous.orientation.x = 0;
    markers_[0].geometry_msg_to_previous.orientation.y = 0;
    markers_[0].geometry_msg_to_previous.orientation.z = 0;
    markers_[0].geometry_msg_to_previous.orientation.w = 1;

    // Transformation Pose to TF
    tf::Vector3 position;
    position.setX(0);
    position.setY(0);
    position.setZ(0);

    tf::Quaternion rotation;
    rotation.setX(0);
    rotation.setY(0);
    rotation.setZ(0);
    rotation.setW(1);

    markers_[0].tf_to_previous.setOrigin(position);
    markers_[0].tf_to_previous.setRotation(rotation);

    // Relative position of first marker equals Global position
    markers_[0].tf_to_world=markers_[0].tf_to_previous;

    // Increase count
    marker_counter_++;

    // Set sign of visibility of first marker
    markers_[0].visible=true;

    ROS_INFO_STREAM("First marker with ID: " << markers_[0].marker_id << " detected");

    //First marker does not have any previous marker
    markers_[0].previous_marker_id = THIS_IS_FIRST_MARKER;
  }

  //------------------------------------------------------
  // FOR EVERY MARKER DO
  //------------------------------------------------------
  for(size_t i = 0; i < temp_markers.size();i++)
  {
    int index;
    int current_marker_id = temp_markers[i].id;

    //Draw marker convex, ID, cube and axis
    temp_markers[i].draw(output_image, cv::Scalar(0,0,255),2);
    aruco::CvDrawingUtils::draw3dCube(output_image,temp_markers[i], aruco_calib_params_);
    aruco::CvDrawingUtils::draw3dAxis(output_image,temp_markers[i], aruco_calib_params_);

    // Existing marker ?
    bool existing = false;
    int temp_counter = 0;

    while((existing == false) && (temp_counter < marker_counter_))
    {
      if(markers_[temp_counter].marker_id == current_marker_id)
      {
        index = temp_counter;
        existing = true;
        ROS_DEBUG_STREAM("Existing marker with ID: " << current_marker_id << "found");
      }
        temp_counter++;
    }

    //New marker ?
    if(existing == false)
    {
      index = marker_counter_;
      markers_[index].marker_id = current_marker_id;
      existing = true;
      ROS_DEBUG_STREAM("New marker with ID: " << current_marker_id << " found");
    }

    // Change visibility flag of new marker
    for(size_t j = 0;j < marker_counter_; j++)
    {
      for(size_t k = 0;k < temp_markers.size(); k++)
      {
        if(markers_[j].marker_id == temp_markers[k].id)
          markers_[j].visible = true;
      }
    }

    //------------------------------------------------------
    // For existing marker do
    //------------------------------------------------------
    if((index < marker_counter_) && (first_marker_detected_ == true))
    {
      markers_[index].current_camera_tf = arucoMarker2Tf(temp_markers[i]);
      markers_[index].current_camera_tf = markers_[index].current_camera_tf.inverse();

      const tf::Vector3 marker_origin = markers_[index].current_camera_tf.getOrigin();
      markers_[index].current_camera_pose.position.x = marker_origin.getX();
      markers_[index].current_camera_pose.position.y = marker_origin.getY();
      markers_[index].current_camera_pose.position.z = marker_origin.getZ();

      const tf::Quaternion marker_quaternion = markers_[index].current_camera_tf.getRotation();
      markers_[index].current_camera_pose.orientation.x = marker_quaternion.getX();
      markers_[index].current_camera_pose.orientation.y = marker_quaternion.getY();
      markers_[index].current_camera_pose.orientation.z = marker_quaternion.getZ();
      markers_[index].current_camera_pose.orientation.w = marker_quaternion.getW();
    }

    //------------------------------------------------------
    // For new marker do
    //------------------------------------------------------
    if((index == marker_counter_) && (first_marker_detected_ == true))
    {
      markers_[index].current_camera_tf=arucoMarker2Tf(temp_markers[i]);

      tf::Vector3 marker_origin = markers_[index].current_camera_tf.getOrigin();
      markers_[index].current_camera_pose.position.x = marker_origin.getX();
      markers_[index].current_camera_pose.position.y = marker_origin.getY();
      markers_[index].current_camera_pose.position.z = marker_origin.getZ();

      tf::Quaternion marker_quaternion = markers_[index].current_camera_tf.getRotation();
      markers_[index].current_camera_pose.orientation.x = marker_quaternion.getX();
      markers_[index].current_camera_pose.orientation.y = marker_quaternion.getY();
      markers_[index].current_camera_pose.orientation.z = marker_quaternion.getZ();
      markers_[index].current_camera_pose.orientation.w = marker_quaternion.getW();

      // Naming - TFs
      std::stringstream camera_tf_id;
      std::stringstream camera_tf_id_old;
      std::stringstream marker_tf_id_old;

      camera_tf_id << "camera_" << index;

      // Flag to keep info if any_known marker_visible in actual image
      bool any_known_marker_visible = false;

      // Array ID of markers, which position of new marker is calculated
      int last_marker_id;

      // Testing, if is possible calculate position of a new marker to old known marker
      for(int k = 0; k < index; k++)
      {
        if((markers_[k].visible == true) && (any_known_marker_visible == false))
        {
          if(markers_[k].previous_marker_id != -1)
          {
            any_known_marker_visible = true;
            camera_tf_id_old << "camera_" << k;
            marker_tf_id_old << "marker_" << k;
            markers_[index].previous_marker_id = k;
            last_marker_id = k;
           }
         }
       }

     // New position can be calculated
     if(any_known_marker_visible == true)
     {
       // Generating TFs for listener
       for(char k = 0; k < 10; k++)
       {
         // TF from old marker and its camera
         broadcaster_.sendTransform(tf::StampedTransform(markers_[last_marker_id].current_camera_tf,ros::Time::now(),
                                                         marker_tf_id_old.str(),camera_tf_id_old.str()));

         // TF from old camera to new camera
         broadcaster_.sendTransform(tf::StampedTransform(markers_[index].current_camera_tf,ros::Time::now(),
                                                         camera_tf_id_old.str(),camera_tf_id.str()));

         ros::Duration(BROADCAST_WAIT_INTERVAL).sleep();
       }

        // Calculate TF between two markers
        listener_->waitForTransform(marker_tf_id_old.str(),camera_tf_id.str(),ros::Time(0),
                                    ros::Duration(WAIT_FOR_TRANSFORM_INTERVAL));
        try
        {
          broadcaster_.sendTransform(tf::StampedTransform(markers_[last_marker_id].current_camera_tf,ros::Time::now(),
                                                          marker_tf_id_old.str(),camera_tf_id_old.str()));

          broadcaster_.sendTransform(tf::StampedTransform(markers_[index].current_camera_tf,ros::Time::now(),
                                                          camera_tf_id_old.str(),camera_tf_id.str()));

          listener_->lookupTransform(marker_tf_id_old.str(),camera_tf_id.str(),ros::Time(0),
                                     markers_[index].tf_to_previous);
        }
        catch(tf::TransformException &e)
        {
          ROS_ERROR("Not able to lookup transform");
        }

        // Save origin and quaternion of calculated TF
        marker_origin = markers_[index].tf_to_previous.getOrigin();
        marker_quaternion = markers_[index].tf_to_previous.getRotation();

        // If plane type selected roll, pitch and Z axis are zero
        if(space_type_ == "plane")
        {
          double roll, pitch, yaw;
          tf::Matrix3x3(marker_quaternion).getRPY(roll,pitch,yaw);
          roll = 0;
          pitch = 0;
          marker_origin.setZ(0);
          marker_quaternion.setRPY(pitch,roll,yaw);
        }

        markers_[index].tf_to_previous.setRotation(marker_quaternion);
        markers_[index].tf_to_previous.setOrigin(marker_origin);

        marker_origin = markers_[index].tf_to_previous.getOrigin();
        markers_[index].geometry_msg_to_previous.position.x = marker_origin.getX();
        markers_[index].geometry_msg_to_previous.position.y = marker_origin.getY();
        markers_[index].geometry_msg_to_previous.position.z = marker_origin.getZ();

        marker_quaternion = markers_[index].tf_to_previous.getRotation();
        markers_[index].geometry_msg_to_previous.orientation.x = marker_quaternion.getX();
        markers_[index].geometry_msg_to_previous.orientation.y = marker_quaternion.getY();
        markers_[index].geometry_msg_to_previous.orientation.z = marker_quaternion.getZ();
        markers_[index].geometry_msg_to_previous.orientation.w = marker_quaternion.getW();

        // increase marker count
        marker_counter_++;

        // Invert and position of new marker to compute camera pose above it
        markers_[index].current_camera_tf = markers_[index].current_camera_tf.inverse();

        marker_origin = markers_[index].current_camera_tf.getOrigin();
        markers_[index].current_camera_pose.position.x = marker_origin.getX();
        markers_[index].current_camera_pose.position.y = marker_origin.getY();
        markers_[index].current_camera_pose.position.z = marker_origin.getZ();

        marker_quaternion = markers_[index].current_camera_tf.getRotation();
        markers_[index].current_camera_pose.orientation.x = marker_quaternion.getX();
        markers_[index].current_camera_pose.orientation.y = marker_quaternion.getY();
        markers_[index].current_camera_pose.orientation.z = marker_quaternion.getZ();
        markers_[index].current_camera_pose.orientation.w = marker_quaternion.getW();

        // Publish all TFs and markers
        publishTfs(false);
      }
    }

    //------------------------------------------------------
    // Compute global position of new marker
    //------------------------------------------------------
    if((marker_counter_previous_ < marker_counter_) && (first_marker_detected_ == true))
    {
      // Publish all TF five times for listener
      for(char k = 0; k < 5; k++)
        publishTfs(false);

      std::stringstream marker_tf_name;
      marker_tf_name << "marker_" << index;

      listener_->waitForTransform("world",marker_tf_name.str(),ros::Time(0),
                                  ros::Duration(WAIT_FOR_TRANSFORM_INTERVAL));
      try
      {
        listener_->lookupTransform("world",marker_tf_name.str(),ros::Time(0),
                                   markers_[index].tf_to_world);
      }
      catch(tf::TransformException &e)
      {
        ROS_ERROR("Not able to lookup transform");
      }

      // Saving TF to Pose
      const tf::Vector3 marker_origin = markers_[index].tf_to_world.getOrigin();
      markers_[index].geometry_msg_to_world.position.x = marker_origin.getX();
      markers_[index].geometry_msg_to_world.position.y = marker_origin.getY();
      markers_[index].geometry_msg_to_world.position.z = marker_origin.getZ();

      tf::Quaternion marker_quaternion=markers_[index].tf_to_world.getRotation();
      markers_[index].geometry_msg_to_world.orientation.x = marker_quaternion.getX();
      markers_[index].geometry_msg_to_world.orientation.y = marker_quaternion.getY();
      markers_[index].geometry_msg_to_world.orientation.z = marker_quaternion.getZ();
      markers_[index].geometry_msg_to_world.orientation.w = marker_quaternion.getW();
    }
  }

  //------------------------------------------------------
  // Compute which of visible markers is the closest to the camera
  //------------------------------------------------------
  bool any_markers_visible=false;
  int num_of_visible_markers=0;

  if(first_marker_detected_ == true)
  {
    double minimal_distance = INIT_MIN_SIZE_VALUE;
    for(int k = 0; k < num_of_markers_; k++)
    {
      double a,b,c,size;

      // If marker is visible, distance is calculated
      if(markers_[k].visible==true)
      {
        a = markers_[k].current_camera_pose.position.x;
        b = markers_[k].current_camera_pose.position.y;
        c = markers_[k].current_camera_pose.position.z;
        size = std::sqrt((a * a) + (b * b) + (c * c));
        if(size < minimal_distance)
        {
          minimal_distance = size;
          closest_camera_index_ = k;
        }

        any_markers_visible = true;
        num_of_visible_markers++;
      }
    }
  }

  //------------------------------------------------------
  // Publish all known markers
  //------------------------------------------------------
  if(first_marker_detected_ == true)
    publishTfs(true);

  //------------------------------------------------------
  // Compute global camera pose
  //------------------------------------------------------
  if((first_marker_detected_ == true) && (any_markers_visible == true))
  {
    std::stringstream closest_camera_tf_name;
    closest_camera_tf_name << "camera_" << closest_camera_index_;

    listener_->waitForTransform("world",closest_camera_tf_name.str(),ros::Time(0),
                                ros::Duration(WAIT_FOR_TRANSFORM_INTERVAL));
    try
    {
      listener_->lookupTransform("world",closest_camera_tf_name.str(),ros::Time(0),
                                 world_position_transform_);
    }
    catch(tf::TransformException &ex)
    {
      ROS_ERROR("Not able to lookup transform");
    }

    // Saving TF to Pose
    const tf::Vector3 marker_origin = world_position_transform_.getOrigin();
    world_position_geometry_msg_.position.x = marker_origin.getX();
    world_position_geometry_msg_.position.y = marker_origin.getY();
    world_position_geometry_msg_.position.z = marker_origin.getZ();

    tf::Quaternion marker_quaternion = world_position_transform_.getRotation();
    world_position_geometry_msg_.orientation.x = marker_quaternion.getX();
    world_position_geometry_msg_.orientation.y = marker_quaternion.getY();
    world_position_geometry_msg_.orientation.z = marker_quaternion.getZ();
    world_position_geometry_msg_.orientation.w = marker_quaternion.getW();
  }

  //------------------------------------------------------
  // Publish all known markers
  //------------------------------------------------------
  if(first_marker_detected_ == true)
    publishTfs(true);

  //------------------------------------------------------
  // Publish custom marker message
  //------------------------------------------------------
  aruco_mapping::ArucoMarker marker_msg;

  if((any_markers_visible == true))
  {
    marker_msg.header.stamp = ros::Time::now();
    marker_msg.header.frame_id = "world";
    marker_msg.marker_visibile = true;
    marker_msg.num_of_visible_markers = num_of_visible_markers;
    marker_msg.global_camera_pose = world_position_geometry_msg_;
    marker_msg.marker_ids.clear();
    marker_msg.global_marker_poses.clear();
    for(size_t j = 0; j < marker_counter_; j++)
    {
      if(markers_[j].visible == true)
      {
        marker_msg.marker_ids.push_back(markers_[j].marker_id);
        marker_msg.global_marker_poses.push_back(markers_[j].geometry_msg_to_world);       
      }
    }
  }
  else
  {
    marker_msg.header.stamp = ros::Time::now();
    marker_msg.header.frame_id = "world";
    marker_msg.num_of_visible_markers = num_of_visible_markers;
    marker_msg.marker_visibile = false;
    marker_msg.marker_ids.clear();
    marker_msg.global_marker_poses.clear();
  }

  // Publish custom marker msg
  marker_msg_pub_.publish(marker_msg);

  return true;
}