tf::Matrix3x3 JPCT_Math::quatToMatrix(tf::Quaternion q) { tf::Matrix3x3 matrix; float norm = magnitudeSquared(q); float s = (double)norm > 0.0?2.0f / norm:0.0F; float xs = q.x() * s; float ys = q.y() * s; float zs = q.z() * s; float xx = q.x() * xs; float xy = q.x() * ys; float xz = q.x() * zs; float xw = q.w() * xs; float yy = q.y() * ys; float yz = q.y() * zs; float yw = q.w() * ys; float zz = q.z() * zs; float zw = q.w() * zs; matrix[0][0] = 1.0F - (yy + zz); matrix[1][0] = xy - zw; matrix[2][0] = xz + yw; matrix[0][1] = xy + zw; matrix[1][1] = 1.0F - (xx + zz); matrix[2][1] = yz - xw; matrix[0][2] = xz - yw; matrix[1][2] = yz + xw; matrix[2][2] = 1.0F - (xx + yy); return matrix; }
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; } }
/** * Convert tf::Quaternion to string */ template<> std::string toString<tf::Quaternion>(const tf::Quaternion& p_quat) { std::stringstream ss; ss << "(" << p_quat.x() << ", " << p_quat.y() << ", " << p_quat.z() << ", " << p_quat.w() << ")"; return ss.str(); }
void ATTCallback (IvyClientPtr app, void* data , int argc, char **argv) { double att_unit_coef= 0.0139882; double phi, theta, yaw; sscanf(argv[0],"%lf %lf %lf %*d %*d %*d",&phi,&theta,&yaw); phi*=att_unit_coef*DEG2RAD; theta*=-att_unit_coef*DEG2RAD; yaw*=-att_unit_coef*DEG2RAD; q.setRPY(phi,theta,yaw); //ROS_INFO("Phi %f; Theta %f; Yaw %f", phi,theta,yaw); //ROS_INFO("q1 %f; q2 %f; q3 %f; q4 %f", q.x(),q.y(),q.z(),q.w()); imu_data.header.stamp = ros::Time::now(); imu_data.orientation.x=q.x(); imu_data.orientation.y=q.y(); imu_data.orientation.z=q.z(); imu_data.orientation.w=q.w(); imu_message.publish(imu_data); //Only temporary until the rates are equal att_data.header.stamp = ros::Time::now(); att_data.orientation.x=q.x(); att_data.orientation.y=q.y(); att_data.orientation.z=q.z(); att_data.orientation.w=q.w(); att_message.publish(att_data); }
/** * Packs current state in a odom message. Needs a quaternion for conversion. */ void pack_pose(tf::Quaternion& q, nav_msgs::Odometry& odom) { q.setRPY(0, 0, _theta); odom.header.stamp = ros::Time::now(); odom.pose.pose.position.x = _x; odom.pose.pose.position.y = _y; odom.pose.pose.orientation.x = q.x(); odom.pose.pose.orientation.y = q.y(); odom.pose.pose.orientation.z = q.z(); odom.pose.pose.orientation.w = q.w(); }
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); } } }
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) { double r, p ,y; lastQuat = tf::Quaternion(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w); btMatrix3x3(lastQuat).getRPY(r, p, y); lastQuat.setRPY(r, p, y + M_PI / 2.0); lastOdom = *msg; }
void imuMsgCallback(const sensor_msgs::Imu& imu_msg) { tf::quaternionMsgToTF(imu_msg.orientation, tmp_); tfScalar yaw, pitch, roll; tf::Matrix3x3(tmp_).getRPY(roll, pitch, yaw); tmp_.setRPY(roll, pitch, 0.0); transform_.setRotation(tmp_); transform_.stamp_ = imu_msg.header.stamp; tfB_->sendTransform(transform_); }
void LeapMotionListener::leapmotionCallback(const leap_motion::leapros2::ConstPtr& dataHand) { dataHand_=(*dataHand); ROS_INFO("ORIENTATION OF THE HAND SET TO \n X: %f\n Y: %f\n Z: %f\n ",dataHand_.ypr.x,dataHand_.ypr.y,dataHand_.ypr.z); ROS_INFO("DIRECTION OF THE HAND SET TO \n X: %f\n Y: %f\n Z: %f\n ",dataHand_.direction.x,dataHand_.direction.y,dataHand_.direction.z); ROS_INFO("NORMAL OF THE HAND SET TO \n X: %f\n Y: %f\n Z: %f\n ",dataHand_.normal.x,dataHand_.normal.y,dataHand_.normal.z); //ROS_INFO("INSIDE CALLBACK"); //We create the values of reference for the first postion of our hand if (FIRST_VALUE) { dataLastHand_.palmpos.x=dataHand_.palmpos.x; dataLastHand_.palmpos.y=dataHand_.palmpos.y; dataLastHand_.palmpos.z=dataHand_.palmpos.z; FIRST_VALUE=0; Updifferencex=dataLastHand_.palmpos.x+10; Downdifferencex=dataLastHand_.palmpos.x-10; Updifferencez=dataLastHand_.palmpos.z+10; Downdifferencez=dataLastHand_.palmpos.z-20; Updifferencey=dataLastHand_.palmpos.y+20; Downdifferencey=dataLastHand_.palmpos.y-20; //ROS_INFO("ORIGINAL POSITION OF THE HAND SET TO \n X: %f\n Y: %f\n Z: %f\n ",trajectory_hand.at(i).palmpos.x,trajectory_hand.at(i).palmpos.y,trajectory_hand.at(i).palmpos.z); //sleep(2); } else { //We get the distance between the finger and transform it into gripper distance rot7=DtA+DtAx*dataHand_.finger_distance; rot8=DtA+DtAx*dataHand_.finger_distance; joint_msg_leap=jointstate_; joint_msg_leap.position[7] = -rot8; joint_msg_leap.position[6] = rot7; if ((dataHand_.palmpos.x<Downdifferencex)||(dataHand_.palmpos.x>Updifferencex)||(dataHand_.palmpos.y<Downdifferencey)||(dataHand_.palmpos.y>Updifferencey)||(dataHand_.palmpos.z<Downdifferencez)||(dataHand_.palmpos.z>Updifferencez)) { //q.setRPY(0,0,M_PI/2);//Fixed Position for testing..with this pose the robot is oriented to the ground q.setRPY(dataHand_.ypr.x,dataHand_.ypr.y,dataHand_.ypr.z); pose.orientation.x = q.getAxis().getX();//cambiado aposta getZ() pose.orientation.y = q.getAxis().getY(); pose.orientation.z = q.getAxis().getZ();//cambiado aposta getX() pose.orientation.w = q.getW(); //pose.orientation.w = ; //pose.orientation.z=1; //pose.orientation.y=0; //pose.orientation.x=0; //We need to send the correct axis to the robot. Currently they are rotated and x is z //ROS_INFO("VALUES OF THE QUATERNION SET TO \n X: %f\n Y: %f\n Z: %f W: %f\n",pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w); pose.position.y +=(dataHand_.palmpos.x-dataLastHand_.palmpos.x) ; pose.position.z +=(dataHand_.palmpos.y-dataLastHand_.palmpos.y); if(pose.position.z>Uplimitez) pose.position.z=Uplimitez; pose.position.x +=(dataHand_.palmpos.z-dataLastHand_.palmpos.z); //Here we instantiate an autogenerated service class srv.request.target = pose ; if (pclient->call(srv)) { //ROS_INFO("Ret: %d", (int)srv.response.ret); dataLastHand_.palmpos.x=dataHand_.palmpos.x; dataLastHand_.palmpos.y=dataHand_.palmpos.y; dataLastHand_.palmpos.z=dataHand_.palmpos.z; // Both limits for x,y,z to avoid small changes Updifferencex=dataLastHand_.palmpos.x+10;// Downdifferencex=dataLastHand_.palmpos.x-10; Updifferencez=dataLastHand_.palmpos.z+10; Downdifferencez=dataLastHand_.palmpos.z-20; Updifferencey=dataLastHand_.palmpos.y+20; Downdifferencey=dataLastHand_.palmpos.y-20; old_pose=pose; ROS_INFO("response %f\n",srv.response.ret); } else { ROS_ERROR("Position out of range"); pose=old_pose; } } //We get the aswer of the service and publish it into the joint_msg_leap message //joint_msg_leap.position[0] = srv.response.joint1; //joint_msg_leap.position[1] = srv.response.joint2; //joint_msg_leap.position[2] = srv.response.joint3; //joint_msg_leap.position[3] = srv.response.joint4; //joint_msg_leap.position[4] = srv.response.joint5; //joint_msg_leap.position[5] = srv.response.joint6; //robo_pub.publish(joint_msg_leap); } //ROS_INFO("END EFFECTOR POSITION \n X: %f\n Y: %f\n Z: %f\n", pose.position.x,pose.position.y,pose.position.z); //ROS_INFO("ORIENTATION OF THE HAND SET TO \n X: %f\n Y: %f\n Z: %f\n ",q.getAxis().getX(),q.getAxis().getY(),q.getAxis().getZ()); }
int main(int argc, char** argv) { ros::init(argc, argv, "test2d"); ros::NodeHandle node; tf::TransformListener t(ros::Duration(20)); tf::StampedTransform tr_o, tr_i; double a_test(0); double b_test(0); double theta_test(0); double nu_theta(1); double nu_trans(1); ROS_INFO_STREAM("waiting for initial transforms"); while (node.ok()) { ros::Time now(ros::Time::now()); //ROS_INFO_STREAM(now); if (t.waitForTransform(baseLinkFrame, now, baseLinkFrame, now, odomFrame, ros::Duration(0.1))) break; //ROS_INFO("wait"); //ros::Duration(0.1).sleep(); } ROS_INFO_STREAM("got first odom to baseLink"); while (node.ok()) { ros::Time now(ros::Time::now()); //ROS_INFO_STREAM(now); if (t.waitForTransform(kinectFrame, now, kinectFrame, now, worldFrame, ros::Duration(0.1))) break; //ROS_INFO("wait"); //ros::Duration(0.1).sleep(); } ROS_INFO_STREAM("got first world to kinect"); sleep(10); ros::Rate rate(0.5); while (node.ok()) { // sleep rate.sleep(); // get parameters from transforms ros::Time curTime(ros::Time::now()); ros::Time lastTime = curTime - ros::Duration(10); //ROS_INFO_STREAM("curTime: " << curTime << ", lastTime: " << lastTime); t.waitForTransform(baseLinkFrame, curTime, baseLinkFrame, lastTime, odomFrame, ros::Duration(3)); t.waitForTransform(kinectFrame, curTime, kinectFrame, lastTime, worldFrame, ros::Duration(3)); t.lookupTransform(baseLinkFrame, curTime, baseLinkFrame, lastTime, odomFrame, tr_o); //ROS_INFO_STREAM("odom to baselink: trans: " << tr_o.getOrigin() << ", rot: " << tr_o.getRotation()); const double alpha_o_tf = tr_o.getOrigin().x(); const double beta_o_tf = tr_o.getOrigin().y(); const double theta_o = 2*atan2(tr_o.getRotation().z(), tr_o.getRotation().w()); const double alpha_o = cos(theta_o)*alpha_o_tf + sin(theta_o)*beta_o_tf; const double beta_o = -sin(theta_o)*alpha_o_tf + cos(theta_o)*beta_o_tf; t.lookupTransform(kinectFrame, curTime, kinectFrame, lastTime, worldFrame, tr_i); //ROS_INFO_STREAM("world to kinect: trans: " << tr_i.getOrigin() << ", rot: " << tr_i.getRotation()); const double alpha_i_tf = tr_i.getOrigin().z(); const double beta_i_tf = -tr_i.getOrigin().x(); const double theta_i = 2*atan2(-tr_i.getRotation().y(), tr_i.getRotation().w()); const double alpha_i = cos(theta_i)*alpha_i_tf + sin(theta_i)*beta_i_tf; const double beta_i = -sin(theta_i)*alpha_i_tf + cos(theta_i)*beta_i_tf; lastTime = curTime; ROS_WARN_STREAM("Input odom: ("<<alpha_o<<", "<<beta_o<<", "<<theta_o<<"), icp: ("\ <<alpha_i<<", "<<beta_i<<", "<<theta_i<<")"); if (abs(theta_i-theta_o) > max_diff_angle) { ROS_WARN_STREAM("Angle difference too big: " << abs(theta_i-theta_o)); continue; } // compute correspondances, check values to prevent numerical instabilities const double R_denom = 2 * sin(theta_o); /*if (abs(R_denom) < limit_low) { ROS_WARN_STREAM("magnitude of R_denom too low: " << R_denom); continue; }*/ const double kr_1 = (alpha_o * cos(theta_o) + alpha_o + beta_o * sin(theta_o)); const double kr_2 = (beta_o * cos(theta_o) + beta_o - alpha_o * sin(theta_o)); const double phi = atan2(kr_2, kr_1); const double r_1 = kr_1/R_denom; const double r_2 = kr_2/R_denom; const double R = sqrt(r_1*r_1 + r_2*r_2); const double kC_1 = (beta_i + beta_i * cos(theta_o) + alpha_i * sin(theta_o)); const double kC_2 = (alpha_i + alpha_i * cos(theta_o) + beta_i * sin(theta_o)); //const double xi = atan2(kC_2 + R_denom*b_test, kC_1 + R_denom*a_test); const double C_1 = kC_1 / R_denom; const double C_2 = kC_2 / R_denom; double xi(0); if (R_denom) xi = atan2(C_1 + a_test, C_2 + b_test); else xi = atan2(kC_1, kC_2); // compute new values //double tmp_theta = M_PI/2 - phi - xi; double tmp_theta = xi - phi; double tmp_a = R * sin(tmp_theta+phi) - C_1; double tmp_b = R * cos(tmp_theta+phi) - C_2; //tmp_theta = (tmp_theta<-M_PI)?tmp_theta+2*M_PI:((tmp_theta>M_PI)?tmp_theta-2*M_PI:tmp_theta); //const double V = sqrt((C_1+a_test)*(C_1+a_test)+(C_2+b_test)*(C_2+b_test)); /*const double err = sqrt((a_test-tmp_a)*(a_test-tmp_a)+(b_test-tmp_b)*(b_test-tmp_b)); const double err_pred = sqrt(R*R + V*V -2*R*V*cos(tmp_theta+phi-xi)); if (abs(err-err_pred)>0.00001) { ROS_WARN_STREAM("Error="<<err<<" Computed="<<err_pred); ROS_WARN_STREAM("chosen: ("<<tmp_a<<", "<<tmp_b<<"); rejected: (" <<R*sin(tmp_theta+phi+M_PI)-C_1<<", "<<R*cos(tmp_theta+phi+M_PI)-C_2<<")"); ROS_WARN_STREAM("R: "<<R<<", V: "<<V<<", C_1: "<<C_1<<", C_2: "<<C_2\ <<", phi: "<<phi<<", xi: "<<xi<<", theta: "<<tmp_theta); }*/ if (R>min_R_rot) { theta_test = atan2((1-nu_theta)*sin(theta_test)+nu_theta*sin(tmp_theta), (1-nu_theta)*cos(theta_test)+nu_theta*cos(tmp_theta)); nu_theta = max(min_nu, 1/(1+1/nu_theta)); } if (R<max_R_trans) { a_test = (1-nu_trans)*a_test + nu_trans*tmp_a; b_test = (1-nu_trans)*b_test + nu_trans*tmp_b; nu_trans = max(min_nu, 1/(1+1/nu_trans)); } // compute transform const tf::Quaternion quat_trans = tf::Quaternion(a_test, b_test, 0, 1); const tf::Quaternion quat_test = tf::Quaternion(0, 0, sin(theta_test/2), cos(theta_test / 2)); const tf::Quaternion quat_axes = tf::Quaternion(-0.5, 0.5, -0.5, 0.5); const tf::Quaternion quat_rot = quat_test*quat_axes; tf::Quaternion quat_tmp = quat_rot.inverse()*quat_trans*quat_rot; const tf::Vector3 vect_trans = tf::Vector3(quat_tmp.x(), quat_tmp.y(), 0); tf::Transform transform; transform.setRotation(quat_rot); transform.setOrigin(vect_trans); ROS_INFO_STREAM("Estimated transform: trans: " << a_test << ", " << b_test << ", rot: " << 2*atan2(quat_test.z(), quat_test.w())); static tf::TransformBroadcaster br; br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), baseLinkFrame, myKinectFrame)); } return 0; }
void InteractiveMarkerArrow::MakeInteractiveMarker(std::string intMarkerName, tf::Quaternion qx_control, tf::Quaternion qy_control, tf::Quaternion qz_control) { visualization_msgs::InteractiveMarker int_marker; int_marker.header.frame_id = "world"; int_marker.scale = 0.1; int_marker.name = intMarkerName; geometry_msgs::Pose pose; tfSrv->get("world", intMarkerName + "_control", pose); int_marker.pose = pose; InteractiveMarkerArrow::MakeControl(int_marker); int_marker.controls[0].interaction_mode = 7; visualization_msgs::InteractiveMarkerControl control; control.orientation_mode = visualization_msgs::InteractiveMarkerControl::FIXED; tf::Transform tr; tfSrv->get("world", "viceGripRotation", tr); qx_control = tr.getRotation() * qx_control; qy_control = tr.getRotation() * qy_control; qz_control = tr.getRotation() * qz_control; control.orientation.w = qx_control.getW(); control.orientation.x = qx_control.getX(); control.orientation.y = qx_control.getY(); control.orientation.z = qx_control.getZ(); control.name = "rotate_x"; control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; int_marker.controls.push_back(control); control.name = "move_x"; control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; int_marker.controls.push_back(control); control.orientation.w = qz_control.getW(); control.orientation.x = qz_control.getX(); control.orientation.y = qz_control.getY(); control.orientation.z = qz_control.getZ(); control.name = "rotate_z"; control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; int_marker.controls.push_back(control); control.name = "move_z"; control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; int_marker.controls.push_back(control); control.orientation.w = qy_control.getW(); control.orientation.x = qy_control.getX(); control.orientation.y = qy_control.getY(); control.orientation.z = qy_control.getZ(); control.name = "rotate_y"; control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; int_marker.controls.push_back(control); control.name = "move_y"; control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; int_marker.controls.push_back(control); intMarkerSrv->insert(int_marker); intMarkerSrv->setCallback(int_marker.name, _processFeedBackTemp(boost::bind(&InteractiveMarkerArrow::ProcessFeedback, this, _1))); intMarkerSrv->applyChanges(); }
// callback for the complete message void complete_message_callback(const homog_track::HomogComplete& msg) { /********** Begin splitting up the incoming message *********/ // getting boolean indicating the reference has been set reference_set = msg.reference_set; // if the reference is set then will break out the points if (reference_set) { // initializer temp scalar to zero temp_scalar = cv::Mat::zeros(1,1,CV_64F); // getting the current marker points circles_curr = msg.current_points; // getting the refernce marker points circles_ref = msg.reference_points; // setting the current points to the point vector curr_red_p.x = circles_curr.red_circle.x; curr_green_p.x = circles_curr.green_circle.x; curr_cyan_p.x = circles_curr.cyan_circle.x; curr_purple_p.x = circles_curr.purple_circle.x; curr_red_p.y = circles_curr.red_circle.y; curr_green_p.y = circles_curr.green_circle.y; curr_cyan_p.y = circles_curr.cyan_circle.y; curr_purple_p.y = circles_curr.purple_circle.y; curr_points_p.push_back(curr_red_p); curr_points_p.push_back(curr_green_p); curr_points_p.push_back(curr_cyan_p); curr_points_p.push_back(curr_purple_p); // converting the points to be the projective coordinates for (int ii = 0; ii < curr_points_m.size(); ii++) { curr_points_m[ii] = K.inv(cv::DECOMP_LU)*curr_points_m[ii]; std::cout << "currpoints at " << ii << " is: " << curr_points_m[ii] << std::endl; } // setting the reference points to the point vector ref_red_p.x = circles_ref.red_circle.x; ref_green_p.x = circles_ref.green_circle.x; ref_cyan_p.x = circles_ref.cyan_circle.x; ref_purple_p.x = circles_ref.purple_circle.x; ref_red_p.y = circles_ref.red_circle.y; ref_green_p.y = circles_ref.green_circle.y; ref_cyan_p.y = circles_ref.cyan_circle.y; ref_purple_p.y = circles_ref.purple_circle.y; ref_points_p.push_back(ref_red_p); ref_points_p.push_back(ref_green_p); ref_points_p.push_back(ref_cyan_p); ref_points_p.push_back(ref_purple_p); // setting the reference points to the matrix vector, dont need to do the last one because its already 1 ref_red_m.at<double>(0,0) = ref_red_p.x; ref_red_m.at<double>(1,0) = ref_red_p.y; ref_green_m.at<double>(0,0) = ref_green_p.x; ref_green_m.at<double>(1,0) = ref_green_p.y; ref_cyan_m.at<double>(0,0) = ref_cyan_p.x; ref_cyan_m.at<double>(1,0) = ref_cyan_p.y; ref_purple_m.at<double>(0,0) = ref_purple_p.x; ref_purple_m.at<double>(1,0) = ref_purple_p.y; ref_points_m.push_back(ref_red_m); ref_points_m.push_back(ref_green_m); ref_points_m.push_back(ref_cyan_m); ref_points_m.push_back(ref_purple_m); // converting the points to be the projective coordinates for (int ii = 0; ii < ref_points_m.size(); ii++) { ref_points_m[ii] = K.inv(cv::DECOMP_LU)*ref_points_m[ii]; //std::cout << "refpoints at " << ii << " is: " << ref_points_m[ii] << std::endl; } // if any of the points have a -1 will skip over the homography if (curr_red_p.x != -1 && curr_green_p.x != -1 && curr_cyan_p.x != -1 && curr_purple_p.x != -1) { //std::cout << "hi" << std::endl; // finding the perspective homography G = cv::findHomography(curr_points_p,ref_points_p,0); //G = cv::findHomography(ref_points_p,ref_points_p,0); std::cout << "G: " << G << std::endl; // decomposing the homography into the four solutions // G and K are 3x3 // R is 3x3 // 3x1 // 3x1 // successful_decomp is the number of solutions found successful_decomp = cv::decomposeHomographyMat(G,K,R,T,n); std::cout << "successful_decomp: " << successful_decomp << std::endl; // if the decomp is successful will find the best matching if (successful_decomp > 0) { std::cout << std::endl << std::endl << " begin check for visibility" << std::endl; // finding the alphas alpha_red.data = 1/(G.at<double>(2,0)*ref_red_p.x + G.at<double>(2,1)*ref_red_p.y + 1); alpha_green.data = 1/(G.at<double>(2,0)*ref_green_p.x + G.at<double>(2,1)*ref_green_p.y + 1); alpha_cyan.data = 1/(G.at<double>(2,0)*ref_cyan_p.x + G.at<double>(2,1)*ref_cyan_p.y + 1); alpha_purple.data = 1/(G.at<double>(2,0)*ref_purple_p.x + G.at<double>(2,1)*ref_purple_p.y + 1); // finding the solutions that give the positive results for (int ii = 0; ii < successful_decomp; ii++) { std::cout << "solution set number " << ii << std::endl; // performing the operation transpose(m)*R*n to check if greater than 0 later // order operating on is red green cyan purple for (int jj = 0; jj < 4; jj++) { //std::cout << " T size: " << T[ii].size() << std::endl; //std::cout << " T type: " << T[ii].type() << std::endl; std::cout << " T value: " << T[ii] << std::endl; //std::cout << " temp scalar 1 " << std::endl; //std::cout << " temp scalar size: " << temp_scalar.size() << std::endl; //std::cout << " temp scalar type: " << temp_scalar.type() << std::endl; //std::cout << " temp scalar value " << temp_scalar <<std::endl; temp_scalar = curr_points_m[jj].t(); //std::cout << " temp scalar 2 " << std::endl; //std::cout << " temp scalar size: " << temp_scalar.size() << std::endl; //std::cout << " temp scalar type: " << temp_scalar.type() << std::endl; //std::cout << " temp scalar value " << temp_scalar <<std::endl; //std::cout << " R size: " << R[ii].size() << std::endl; //std::cout << " R type: " << R[ii].type() << std::endl; //std::cout << " R value: " << R[ii] << std::endl; temp_scalar = temp_scalar*R[ii]; //std::cout << " temp scalar 3 " << std::endl; //std::cout << " temp scalar size: " << temp_scalar.size() << std::endl; //std::cout << " temp scalar type: " << temp_scalar.type() << std::endl; //std::cout << " temp scalar value " << temp_scalar <<std::endl; //std::cout << " n size: " << n[ii].size() << std::endl; //std::cout << " n type: " << n[ii].type() << std::endl; std::cout << " n value: " << n[ii] << std::endl; temp_scalar = temp_scalar*n[ii]; //std::cout << " temp scalar size: " << temp_scalar.size() << std::endl; //std::cout << " temp scalar type: " << temp_scalar.type() << std::endl; //std::cout << " temp scalar value " << temp_scalar <<std::endl; //std::cout << " temp scalar value at 0,0" << temp_scalar.at<double>(0,0) << std::endl; scalar_value_check.push_back(temp_scalar.at<double>(0,0)); ////std::cout << " scalar value check size: " << scalar_value_check.size() << std::endl; //std::cout << " \tthe value for the " << jj << " visibility check is: " << scalar_value_check[4*ii+jj] << std::endl; } } std::cout << " end check for visibility" << std::endl << std::endl; // restting first solution found and second solution found first_solution_found = false; second_solution_found = false; fc_found = false; // getting the two solutions or only one if there are not two for (int ii = 0; ii < successful_decomp; ii++) { // getting the values onto the temporary vector // getting the start and end of the next solution temp_solution_start = scalar_value_check.begin() + 4*ii; temp_solution_end = scalar_value_check.begin() + 4*ii+4; temp_solution.assign(temp_solution_start,temp_solution_end); // checking if all the values are positive all_positive = true; current_temp_index = 0; while (all_positive && current_temp_index < 4) { if (temp_solution[current_temp_index] >= 0) { current_temp_index++; } else { all_positive = false; } } // if all the values were positive and a first solution has not been found will assign // to first solution. if all positive and first solution has been found will assign // to second solution. if all positive is false then will not do anything if (all_positive && first_solution_found && !second_solution_found) { // setting it to indicate a solution has been found second_solution_found = true; // setting the rotation, translation, and normal to be the second set second_R = R[ii]; second_T = T[ii]; second_n = n[ii]; // setting the projected values second_solution = temp_solution; } else if (all_positive && !first_solution_found) { // setting it to indicate a solution has been found first_solution_found = true; // setting the rotation, translation, and normal to be the first set first_R = R[ii]; first_T = T[ii]; first_n = n[ii]; // setting the projected values first_solution = temp_solution; } // erasing all the values from the temp solution temp_solution.erase(temp_solution.begin(),temp_solution.end()); } // erasing all the scalar values from the check scalar_value_check.erase(scalar_value_check.begin(),scalar_value_check.end()); // displaying the first solution if it was found if (first_solution_found) { std::cout << std::endl << "first R: " << first_R << std::endl; std::cout << "first T: " << first_T << std::endl; std::cout << "first n: " << first_n << std::endl; for (double ii : first_solution) { std::cout << ii << " "; } std::cout << std::endl; } // displaying the second solution if it was found if (second_solution_found) { std::cout << std::endl << "second R: " << second_R << std::endl; std::cout << "second T: " << second_T << std::endl; std::cout << "second n: " << second_n << std::endl; for (double ii : second_solution) { std::cout << ii << " "; } std::cout << std::endl; } // because the reference is set to the exact value when when n should have only a z componenet, the correct // choice should be the one closest to n_ref = [0,0,1]^T which will be the one with the greatest dot product with n_ref if (first_solution_found && second_solution_found) { if (first_n.dot(n_ref) >= second_n.dot(n_ref)) { R_fc = first_R; T_fc = first_T; } else { R_fc = second_R; T_fc = second_T; } fc_found = true; } else if(first_solution_found) { R_fc = first_R; T_fc = first_T; fc_found = true; } //if a solution was found will publish // need to convert to pose message so use if (fc_found) { // converting the rotation from a cv matrix to quaternion, first need it as a matrix3x3 R_fc_tf[0][0] = R_fc.at<double>(0,0); R_fc_tf[0][1] = R_fc.at<double>(0,1); R_fc_tf[0][2] = R_fc.at<double>(0,2); R_fc_tf[1][0] = R_fc.at<double>(1,0); R_fc_tf[1][1] = R_fc.at<double>(1,1); R_fc_tf[1][2] = R_fc.at<double>(1,2); R_fc_tf[2][0] = R_fc.at<double>(2,0); R_fc_tf[2][1] = R_fc.at<double>(2,1); R_fc_tf[2][2] = R_fc.at<double>(2,2); std::cout << "Final R:\n" << R_fc << std::endl; // converting the translation to a vector 3 T_fc_tf.setX(T_fc.at<double>(0,0)); T_fc_tf.setY(T_fc.at<double>(0,1)); T_fc_tf.setZ(T_fc.at<double>(0,2)); std::cout << "Final T :\n" << T_fc << std::endl; // getting the rotation as a quaternion R_fc_tf.getRotation(Q_fc_tf); std::cout << "current orientation:" << "\n\tx:\t" << Q_fc_tf.getX() << "\n\ty:\t" << Q_fc_tf.getY() << "\n\tz:\t" << Q_fc_tf.getZ() << "\n\tw:\t" << Q_fc_tf.getW() << std::endl; std::cout << "norm of quaternion:\t" << Q_fc_tf.length() << std::endl; // getting the negated version of the quaternion for the check Q_fc_tf_negated = tf::Quaternion(-Q_fc_tf.getX(),-Q_fc_tf.getY(),-Q_fc_tf.getZ(),-Q_fc_tf.getW()); std::cout << "negated orientation:" << "\n\tx:\t" << Q_fc_tf_negated.getX() << "\n\ty:\t" << Q_fc_tf_negated.getY() << "\n\tz:\t" << Q_fc_tf_negated.getZ() << "\n\tw:\t" << Q_fc_tf_negated.getW() << std::endl; std::cout << "norm of negated quaternion:\t" << Q_fc_tf_negated.length() << std::endl; // showing the last orientation std::cout << "last orientation:" << "\n\tx:\t" << Q_fc_tf_last.getX() << "\n\ty:\t" << Q_fc_tf_last.getY() << "\n\tz:\t" << Q_fc_tf_last.getZ() << "\n\tw:\t" << Q_fc_tf_last.getW() << std::endl; std::cout << "norm of last quaternion:\t" << Q_fc_tf_last.length() << std::endl; // checking if the quaternion has flipped Q_norm_current_diff = std::sqrt(std::pow(Q_fc_tf.getX() - Q_fc_tf_last.getX(),2.0) + std::pow(Q_fc_tf.getY() - Q_fc_tf_last.getY(),2.0) + std::pow(Q_fc_tf.getZ() - Q_fc_tf_last.getZ(),2.0) + std::pow(Q_fc_tf.getW() - Q_fc_tf_last.getW(),2.0)); std::cout << "current difference:\t" << Q_norm_current_diff << std::endl; Q_norm_negated_diff = std::sqrt(std::pow(Q_fc_tf_negated.getX() - Q_fc_tf_last.getX(),2.0) + std::pow(Q_fc_tf_negated.getY() - Q_fc_tf_last.getY(),2.0) + std::pow(Q_fc_tf_negated.getZ() - Q_fc_tf_last.getZ(),2.0) + std::pow(Q_fc_tf_negated.getW() - Q_fc_tf_last.getW(),2.0)); std::cout << "negated difference:\t" << Q_norm_negated_diff << std::endl; if (Q_norm_current_diff > Q_norm_negated_diff) { Q_fc_tf = Q_fc_tf_negated; } // updating the last Q_fc_tf_last = Q_fc_tf; // converting the tf quaternion to a geometry message quaternion Q_fc_gm.x = Q_fc_tf.getX(); Q_fc_gm.y = Q_fc_tf.getY(); Q_fc_gm.z = Q_fc_tf.getZ(); Q_fc_gm.w = Q_fc_tf.getW(); // converting the tf vector3 to a point P_fc_gm.x = T_fc_tf.getX(); P_fc_gm.y = T_fc_tf.getY(); P_fc_gm.z = T_fc_tf.getZ(); // setting the transform with the values fc_tf.setOrigin(T_fc_tf); fc_tf.setRotation(Q_fc_tf); tf_broad.sendTransform(tf::StampedTransform(fc_tf, msg.header.stamp,"f_star","f_current")); // setting the decomposed message pose_fc_gm.position = P_fc_gm; pose_fc_gm.orientation = Q_fc_gm; decomposed_msg.pose = pose_fc_gm; decomposed_msg.header.stamp = msg.header.stamp; decomposed_msg.header.frame_id = "current_frame_normalized"; decomposed_msg.alpha_red = alpha_red; decomposed_msg.alpha_green = alpha_green; decomposed_msg.alpha_cyan = alpha_cyan; decomposed_msg.alpha_purple = alpha_purple; homog_decomp_pub.publish(decomposed_msg); std::cout << "complete message\n" << decomposed_msg << std::endl << std::endl; // publish the marker marker.pose = pose_fc_gm; marker_pub.publish(marker); } } } // erasing all the temporary points if (first_solution_found || second_solution_found) { // erasing all the point vectors and matrix vectors curr_points_p.erase(curr_points_p.begin(),curr_points_p.end()); ref_points_p.erase(ref_points_p.begin(),ref_points_p.end()); curr_points_m.erase(curr_points_m.begin(),curr_points_m.end()); ref_points_m.erase(ref_points_m.begin(),ref_points_m.end()); } } /********** End splitting up the incoming message *********/ }
float JPCT_Math::magnitudeSquared(tf::Quaternion q) { return q.w() * q.w() + q.x() * q.x() + q.y() * q.y() + q.z() * q.z(); }
bool Hand_filter::jumped(const tf::Quaternion& q_current,const tf::Quaternion& q_previous,const float threashold) const { tf::Vector3 axis = q_current.getAxis(); // std::cout<< "axis: " << axis.x() << " " << axis.y() << " " << axis.z() << std::endl; return false; }
int main (int argc, char** argv) { ros::init(argc, argv, "tp_serial"); ros::NodeHandle n; ROS_INFO("Serial server is starting"); ros::Time current_time; tf::TransformBroadcaster transform_broadcaster; ros::Subscriber ucCommandMsg; // subscript to "cmd_vel" for receive command ros::Publisher odom_pub; // publish the odometry ros::Publisher path_pub; nav_msgs::Odometry move_base_odom; nav_msgs::Path pathMsg; static tf::Quaternion move_base_quat; //Initialize fixed values for odom and tf message move_base_odom.header.frame_id = "odom"; move_base_odom.child_frame_id = "base_robot"; //Reset all covariance values move_base_odom.pose.covariance = boost::assign::list_of(WHEEL_COVARIANCE)(0)(0)(0)(0)(0) (0)(WHEEL_COVARIANCE)(0)(0)(0)(0) (0)(0)(999999)(0)(0)(0) (0)(0)(0)(999999)(0)(0) (0)(0)(0)(0)(999999)(0) (0)(0)(0)(0)(0)(WHEEL_COVARIANCE); move_base_odom.twist.covariance = boost::assign::list_of(WHEEL_COVARIANCE)(0)(0)(0)(0)(0) (0)(WHEEL_COVARIANCE)(0)(0)(0)(0) (0)(0)(999999)(0)(0)(0) (0)(0)(0)(999999)(0)(0) (0)(0)(0)(0)(999999)(0) (0)(0)(0)(0)(0)(WHEEL_COVARIANCE); pathMsg.header.frame_id = "odom"; //----------ROS Odometry publishing------------------ //----------Initialize serial connection ROS_INFO("Serial Initialing:\n Port: %s \n BaudRate: %d", DEFAULT_SERIALPORT, DEFAULT_BAUDRATE); int fd = -1; struct termios newtio; FILE *fpSerial = NULL; // read/write, not controlling terminal for process fd = open(DEFAULT_SERIALPORT, O_RDWR | O_NOCTTY | O_NDELAY); if (fd < 0) { ROS_ERROR("Serial Init: Could not open serial device %s", DEFAULT_SERIALPORT); return 0; } //set up new setting memset(&newtio, 0, sizeof(newtio)); newtio.c_cflag = CS8 | CLOCAL | CREAD; //8bit, no parity, 1 stop bit newtio.c_iflag |= IGNBRK; //ignore break codition newtio.c_oflag = 0; //all options off //newtio.c_lflag = ICANON; //process input as lines newtio.c_cc[VTIME] = 0; newtio.c_cc[VMIN] = 20; // byte readed per a time //active new setting tcflush(fd, TCIFLUSH); if (cfsetispeed(&newtio, BAUDMACRO) < 0 || cfsetospeed(&newtio, BAUDMACRO)) { ROS_ERROR("Serial Init: Failed to set serial BaudRate: %d", DEFAULT_BAUDRATE); close(fd); return 0; } ROS_INFO("Connection established with %s at %d.", DEFAULT_SERIALPORT, BAUDMACRO); tcsetattr(fd, TCSANOW, &newtio); tcflush(fd, TCIOFLUSH); // Open file as a standard I/O stream fpSerial = fdopen(fd, "r+"); if (!fpSerial) { ROS_ERROR("Serial Init: Failed to open serial stream %s", DEFAULT_SERIALPORT); fpSerial = NULL; } //Creating message to talk with ROS //Subscribe to ROS message ucCommandMsg = n.subscribe<geometry_msgs::Twist>("cmd_vel", 1, ucCommandCallback); //Setup to publish ROS message odom_pub = n.advertise<nav_msgs::Odometry>("tp_robot/odom", 10); path_pub = n.advertise<nav_msgs::Path>("tp_robot/odomPath", 10); // An "adaptive" timer to maintain periodical communication with the MCU ros::Rate rate(FPS); uint8_t i = FPS; while (i--) { find_mean = true; rate.sleep(); ros::spinOnce(); } find_mean = false; //Loop for input while(ros::ok()) { //Process the callbacks ros::spinOnce(); //Impose command and get back respone int res; cmd_frame[0] = 'f'; //cmd_frame[1] = 'T'; *(uint16_t *)(cmd_frame + 2) = pkg_id; //*(uint16_t *)(cmd_frame + 4) = left_speed; //set left speed //*(uint16_t *)(cmd_frame + 6) = right_speed; // set right speed res = write(fd, cmd_frame, 8); if (res < 8) ROS_ERROR("Error sending command"); current_time = ros::Time::now(); //sleep to wait the response is returned rate.sleep(); //Read a number of RCV_LENGHT chars as if they have arrived res = read(fd, rsp_frame, RCV_LENGTH); if (res < RCV_LENGTH) { //ROS_INFO("frame %d dropped", pkg_id); while(read(fd, rsp_frame, 1) > 0); } else { uint16_t rcv_pkg_id = *(uint16_t *)(rsp_frame +2); if(rcv_pkg_id == pkg_id) { //read the commands to display cmd_field[0] = rsp_frame[0]; cmd_field[1] = rsp_frame[1]; cmd_field[2] = 0; //Time sent from RTOS to check if the moving base has been reset uint8_t reset_robot = *(uint8_t *)(rsp_frame + 16); //Calculate the x,y,z,v,w odometry data double left_speed = *(int16_t *)(rsp_frame + 4)*M_PI/98500; double right_speed = *(int16_t *)(rsp_frame + 6)*M_PI/98500; //Get the newly traveled distance on each wheel if (rcv_pkg_id == 0 || reset_robot == 1 ) { left_path_offset = *(int32_t *)(rsp_frame +8)*M_PI/98500; right_path_offset = *(int32_t *)(rsp_frame + 12)*M_PI/98500; left_path = 0; right_path = 0; } else { left_path = *(int32_t *)(rsp_frame +8)*M_PI/98500 - left_path_offset; right_path = *(int32_t *)(rsp_frame + 12)*M_PI/98500 - right_path_offset; } //Calculate and publish the odometry data double left_delta = left_path - left_path1; double right_delta = right_path - right_path1; //only if the robot has traveled a significant distance will be calculate the odometry if(!((left_delta < 0.0075 && left_delta > -0.0075) && (right_delta < 0.0075 && right_delta > -0.0075))) { //Calculate the pose in reference to world base theta += (right_delta - left_delta)/0.3559; //translate theta to [-pi; pi] if(theta > (2*M_PI)) theta -= 2*M_PI; if (theta < 0) theta += 2*M_PI; //Calculate displacement double disp = (right_delta + left_delta)/2; x += disp*cos(theta); y += disp*sin(theta); //Calculate the twist in reference to the robot base linear_x = (right_speed + left_speed)/2; angular_z = (right_speed - left_speed)/0.362; left_path1 = left_path; right_path1 = right_path; } // Infer the rotation angle and publish transform move_base_quat = tf::Quaternion(tf::Vector3(0, 0, 1), theta); transform_broadcaster.sendTransform(tf::StampedTransform( tf::Transform(move_base_quat, tf::Vector3(x,y,0)), current_time, "odom", "base_robot")); //Publish the odometry message over ROS move_base_odom.header.stamp = current_time; //move_base_odom.header.stamp = ros::Time::now(); //set the position move_base_odom.pose.pose.position.x = x; move_base_odom.pose.pose.position.y = y; move_base_odom.pose.pose.position.z = 0; move_base_odom.pose.pose.orientation.x = move_base_quat.x(); move_base_odom.pose.pose.orientation.y = move_base_quat.y(); move_base_odom.pose.pose.orientation.z = move_base_quat.z(); move_base_odom.pose.pose.orientation.w = move_base_quat.w(); //Set the velocity move_base_odom.child_frame_id = "base_robot"; move_base_odom.twist.twist.linear.x = linear_x; move_base_odom.twist.twist.linear.y = 0; move_base_odom.twist.twist.angular.z = angular_z; //Push back the pose to path message pathMsg.header = move_base_odom.header; geometry_msgs::PoseStamped pose_stamped; pose_stamped.header = move_base_odom.header; pose_stamped.pose = move_base_odom.pose.pose; pathMsg.poses.push_back(pose_stamped); //publish the odom and path message odom_pub.publish(move_base_odom); path_pub.publish(pathMsg); //ROS_INFO("MCU responded: %s\t%d\t%f\t%f\t%f\t%f\t%d", cmd_field,\ rcv_pkg_id,\ left_speed,\ right_speed,\ left_path,\ right_path,\ reset_robot); } else { ROS_INFO("frame %d corrupted !", pkg_id); while(read(fd, rsp_frame, 1) > 0); } }
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; }
void MocapKalman::spinOnce( ) { const static double dt = (double)r.expectedCycleTime( ).nsec / 1000000000 + r.expectedCycleTime( ).sec; static double K[36] = { 0 }; static double F[36] = { 0 }; static geometry_msgs::PoseWithCovariance residual; static geometry_msgs::Vector3 rpy; static tf::Quaternion curr_quat; static tf::StampedTransform tr; // Get the transform try { li.lookupTransform( frame_base, frame_id, ros::Time(0), tr ); } catch( tf::TransformException ex ) { ROS_INFO( "Missed a transform...chances are that we are still OK" ); return; } if( tr.getOrigin( ).x( ) != tr.getOrigin( ).x( ) ) { ROS_WARN( "NaN DETECTED" ); return; } nav_msgs::OdometryPtr odom_msg( new nav_msgs::Odometry ); odom_msg->header.frame_id = frame_base; odom_msg->header.stamp = ros::Time::now( ); odom_msg->child_frame_id = frame_id; // Get the RPY from this iteration curr_quat = tr.getRotation( ); tf::Matrix3x3( ( curr_quat * last_quat.inverse( ) ).normalize( ) ).getRPY(rpy.x, rpy.y, rpy.z); // Step 1: // x = F*x // We construct F based on the acceleration previously observed // We will assume that dt hasn't changed much since the last calculation F[0] = ( xdot.twist.linear.x < 0.001 ) ? 1 : ( xdot.twist.linear.x + last_delta_twist.twist.linear.x ) / xdot.twist.linear.x; F[7] = ( xdot.twist.linear.y < 0.001 ) ? 1 : ( xdot.twist.linear.y + last_delta_twist.twist.linear.y ) / xdot.twist.linear.y; F[14] = ( xdot.twist.linear.z < 0.001 ) ? 1 : ( xdot.twist.linear.z + last_delta_twist.twist.linear.z ) / xdot.twist.linear.z; F[21] = ( xdot.twist.angular.x < 0.001 ) ? 1 : ( xdot.twist.angular.x + last_delta_twist.twist.angular.x ) / xdot.twist.angular.x; F[28] = ( xdot.twist.angular.y < 0.001 ) ? 1 : ( xdot.twist.angular.y + last_delta_twist.twist.angular.y ) / xdot.twist.angular.y; F[35] = ( xdot.twist.angular.z < 0.001 ) ? 1 : ( xdot.twist.angular.z + last_delta_twist.twist.angular.z ) / xdot.twist.angular.z; // Step 2: // P = F*P*F' + Q // Since F is only populated on the diagonal, F=F' xdot.covariance[0] += linear_process_variance; xdot.covariance[7] += linear_process_variance; xdot.covariance[14] += linear_process_variance; xdot.covariance[21] += angular_process_variance; xdot.covariance[28] += angular_process_variance; xdot.covariance[35] += angular_process_variance; // Step 3: // y = z - H*x // Because x estimates z directly, H = I residual.pose.position.x = ( tr.getOrigin( ).x( ) - last_transform.getOrigin( ).x( ) ) / dt - xdot.twist.linear.x; residual.pose.position.y = ( tr.getOrigin( ).y( ) - last_transform.getOrigin( ).y( ) ) / dt - xdot.twist.linear.y; residual.pose.position.z = ( tr.getOrigin( ).z( ) - last_transform.getOrigin( ).z( ) ) / dt - xdot.twist.linear.z; // HACK for some odd discontinuity in the rotations... //if( rpy.x > .4 || rpy.y > .4 || rpy.z > .4 || rpy.x < -.4 || rpy.y < -.4 || rpy.z < -.4 ) // rpy.x = rpy.y = rpy.z = 0; residual.pose.orientation.x = rpy.x / dt - xdot.twist.angular.x; residual.pose.orientation.y = rpy.y / dt - xdot.twist.angular.y; residual.pose.orientation.z = rpy.z / dt - xdot.twist.angular.z; // Step 4: // S = H*P*H' + R // Again, since H = I, S is simply P + R residual.covariance[0] = xdot.covariance[0] + linear_observation_variance; residual.covariance[7] = xdot.covariance[7] + linear_observation_variance; residual.covariance[14] = xdot.covariance[14] + linear_observation_variance; residual.covariance[21] = xdot.covariance[21] + angular_observation_variance; residual.covariance[28] = xdot.covariance[28] + angular_observation_variance; residual.covariance[35] = xdot.covariance[35] + angular_observation_variance; // Step 5: // K = P*H'*S^(-1) // Again, since H = I, and since S is only populated along the diagonal, // we can invert each element along the diagonal K[0] = xdot.covariance[0] / residual.covariance[0]; K[7] = xdot.covariance[7] / residual.covariance[7]; K[14] = xdot.covariance[14] / residual.covariance[14]; K[21] = xdot.covariance[21] / residual.covariance[21]; K[28] = xdot.covariance[28] / residual.covariance[28]; K[35] = xdot.covariance[35] / residual.covariance[35]; // Step 6: // x = x + K*y xdot.twist.linear.x += K[0] * residual.pose.position.x; xdot.twist.linear.y += K[7] * residual.pose.position.y; xdot.twist.linear.z += K[14] * residual.pose.position.z; xdot.twist.angular.x += K[21] * residual.pose.orientation.x; xdot.twist.angular.y += K[28] * residual.pose.orientation.y; xdot.twist.angular.z += K[35] * residual.pose.orientation.z; // Step 7: // P = (I - K*H)*P // H is still I, so (I-K) * P xdot.covariance[0] *= -K[0]; xdot.covariance[7] *= -K[7]; xdot.covariance[14] *= -K[14]; xdot.covariance[21] *= -K[21]; xdot.covariance[28] *= -K[28]; xdot.covariance[35] *= -K[35]; // Populate Message odom_msg->pose.pose.position.x = tr.getOrigin( ).x( ); odom_msg->pose.pose.position.y = tr.getOrigin( ).y( ); odom_msg->pose.pose.position.z= tr.getOrigin( ).z( ); tf::quaternionTFToMsg( tr.getRotation( ), odom_msg->pose.pose.orientation ); odom_msg->twist = xdot; if( local_frame ) { // Rotate velocity vector to the local frame tf::Vector3 tmp; tf::Vector3 tmp2; tf::vector3MsgToTF( odom_msg->twist.twist.linear, tmp ); tf::vector3MsgToTF( odom_msg->twist.twist.angular, tmp2 ); tf::vector3TFToMsg( tf::quatRotate( curr_quat.inverse( ), tmp ), odom_msg->twist.twist.linear ); tf::vector3TFToMsg( tf::quatRotate( curr_quat.inverse( ), tmp2 ), odom_msg->twist.twist.angular ); } // Done, Publish odom_pub.publish( odom_msg ); // Record when this message was for next time last_delta_twist.twist.linear.x = odom_msg->twist.twist.linear.x - last_twist.twist.linear.x; last_delta_twist.twist.linear.y = odom_msg->twist.twist.linear.y - last_twist.twist.linear.y; last_delta_twist.twist.linear.z = odom_msg->twist.twist.linear.z - last_twist.twist.linear.z; last_delta_twist.twist.angular.x = odom_msg->twist.twist.angular.x - last_twist.twist.angular.x; last_delta_twist.twist.angular.y = odom_msg->twist.twist.angular.y - last_twist.twist.angular.y; last_delta_twist.twist.angular.z = odom_msg->twist.twist.angular.z - last_twist.twist.angular.z; last_transform = tr; last_twist = odom_msg->twist; last_quat = curr_quat; };