double Controller::positionBaseSpeed(double time, double lin_speed, bool gripperStop)
{
double start_time = ros::Time::now().toSec();
geometry_msgs::Twist msg;
msg.linear.x = lin_speed;
ros::Rate sleep_rate(50);
mGripperStop = false;
while (ros::ok() && ros::Time::now().toSec() - start_time < time && (gripperStop == false || mGripperStop == false))
{
mBaseSpeedPublisher.publish(msg);
sleep_rate.sleep();
ros::spinOnce();
}
msg.linear.x = 0.0;
mBaseSpeedPublisher.publish(msg);
return ros::Time::now().toSec() - start_time;
}
int main( int argc, char** argv) {
// initialize node
ros::init(argc, argv, "marker_calibration");
ros::NodeHandle node_handle;
ros::NodeHandle private_ns_node_handle("~");
unsigned int nr_of_samples=50;
// subscribe to topic
ros::Subscriber ar_pose_marker_subscriber = node_handle.subscribe("/ar_pose_marker", 10, ar_pose_marker_callback);
// wait for
ros::Rate sleep_rate(1.0);
while( ros::ok() && nr_of_samples_received<nr_of_samples ){
ros::spinOnce();
sleep_rate.sleep();
printf("got %i samples\n",nr_of_samples_received);
}
if( nr_of_samples<=nr_of_samples_received ){
printf("received enough markers\n");
printf("marker_0 = %i, marker_1 = %i, marker_2 = %i\n", (int) marker0_pose.size(), (int) marker1_pose.size(), (int) marker2_pose.size());
ROS_INFO("average result");
print_statistic_attributes( marker0_pose );
print_statistic_attributes( marker1_pose );
print_statistic_attributes( marker2_pose );
geometry_msgs::Pose marker0_pose_avg;
marker0_pose_avg.position = calc_position_average( marker0_pose );
marker0_pose_avg.orientation = calc_quaternion_average( marker0_pose );
geometry_msgs::Pose marker1_pose_avg;
marker1_pose_avg.position = calc_position_average( marker1_pose );
marker1_pose_avg.orientation = calc_quaternion_average( marker1_pose );
geometry_msgs::Pose marker2_pose_avg;
marker2_pose_avg.position = calc_position_average( marker2_pose );
marker2_pose_avg.orientation = calc_quaternion_average( marker2_pose );
if( marker0_frame_id.compare(marker1_frame_id)!=0 || marker1_frame_id.compare(marker2_frame_id)!=0 ){
ROS_ERROR("no common axis found");
return -1;
}
tf::Transform marker0_to_common_axis, marker1_to_common_axis, marker2_to_common_axis;
tf::Transform marker0_to_map, marker1_to_map, marker2_to_map;
tf::poseMsgToTF( marker0_pose_avg, marker0_to_common_axis);
tf::poseMsgToTF( marker1_pose_avg, marker1_to_common_axis);
tf::poseMsgToTF( marker2_pose_avg, marker2_to_common_axis);
// calc transformation marker0_to_map
marker0_to_map.getOrigin()[0] = (marker1_to_common_axis.inverse()*marker0_to_common_axis.getOrigin())[2];
marker0_to_map.getOrigin()[1] = 0.0;
marker0_to_map.getOrigin()[2] = (marker2_to_common_axis.inverse()*marker0_to_common_axis.getOrigin())[2];
marker0_to_map.setBasis( tf::Matrix3x3(1, 0, 0, 0, 0, 1, 0,-1, 0));
// calc transformation marker1_to_map
marker1_to_map.getOrigin()[0] = 0.0;
marker1_to_map.getOrigin()[1] = (marker0_to_common_axis.inverse()*marker1_to_common_axis.getOrigin())[2];
marker1_to_map.getOrigin()[2] = (marker2_to_common_axis.inverse()*marker1_to_common_axis.getOrigin())[2];
marker1_to_map.setBasis( tf::Matrix3x3(0, 0, 1, 1, 0, 0, 0, 1, 0));
// calc transformation marker1_to_map
marker2_to_map.getOrigin()[0] = (marker1_to_common_axis.inverse()*marker2_to_common_axis.getOrigin())[2];
marker2_to_map.getOrigin()[1] = (marker0_to_common_axis.inverse()*marker2_to_common_axis.getOrigin())[2];
marker2_to_map.getOrigin()[2] = 0.0;
marker2_to_map.setBasis( tf::Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1));
tf::Transform map_to_common_axis0, map_to_common_axis1, map_to_common_axis2;
map_to_common_axis0 = marker0_to_common_axis*marker0_to_map.inverse();
map_to_common_axis1 = marker1_to_common_axis*marker1_to_map.inverse();
map_to_common_axis2 = marker2_to_common_axis*marker2_to_map.inverse();
printf("marker 2 2 common axis = [%f,%f,%f]\n", marker2_to_common_axis.getOrigin()[0], marker2_to_common_axis.getOrigin()[1], marker2_to_common_axis.getOrigin()[2]);
printf("marker 2 2 map = [%f,%f,%f]\n", marker2_to_map.getOrigin()[0], marker2_to_map.getOrigin()[1], marker2_to_map.getOrigin()[2]);
printf("map 2 marker 2 = [%f,%f,%f]\n", marker2_to_map.inverse().getOrigin()[0], marker2_to_map.inverse().getOrigin()[1], marker2_to_map.inverse().getOrigin()[2]);
printf("map 2 common axis 2 = [%f,%f,%f]\n", map_to_common_axis2.getOrigin()[0], map_to_common_axis2.getOrigin()[1], map_to_common_axis2.getOrigin()[2]);
std::vector<tf::Transform> map_to_common_axis_vector;
map_to_common_axis_vector.push_back( map_to_common_axis0);
map_to_common_axis_vector.push_back( map_to_common_axis1);
map_to_common_axis_vector.push_back( map_to_common_axis2);
tf::Transform map_to_common_axis_avg = calc_pose_average( map_to_common_axis_vector);
static tf::TransformBroadcaster br;
printf("map 2 common axis = [%f,%f,%f]\n", map_to_common_axis_avg.getOrigin()[0], map_to_common_axis_avg.getOrigin()[1], map_to_common_axis_avg.getOrigin()[2]);
printf("map 2 common axis avg = [%f,%f,%f]\n", map_to_common_axis_avg.getOrigin()[0], map_to_common_axis_avg.getOrigin()[1], map_to_common_axis_avg.getOrigin()[2]);
tf::Transform marker0_to_map_avg = map_to_common_axis_avg.inverse()*marker0_to_common_axis;
tf::Transform marker1_to_map_avg = map_to_common_axis_avg.inverse()*marker1_to_common_axis;
tf::Transform marker2_to_map_avg = map_to_common_axis_avg.inverse()*marker2_to_common_axis;
ROS_INFO("save parameters");
std::ofstream yaml_file( "/home/omnirob/catkin_ws/src/omnirob_robin/omnirob_robin_object_detection/data/marker_calibration.yaml" );
yaml_file << "marker_ids: [0, 1, 2]\n";
yaml_file << "marker0:\n"; // <!-- described in map coordinates -->
yaml_file << " position:\n";
yaml_file << " x: " << marker0_to_map_avg.getOrigin()[0] <<"\n";
yaml_file << " y: " << marker0_to_map_avg.getOrigin()[1] <<"\n";
yaml_file << " z: " << marker0_to_map_avg.getOrigin()[2] <<"\n";
yaml_file << " orientation:\n";
yaml_file << " x: " << marker0_to_map_avg.getRotation()[0] <<"\n";
yaml_file << " y: " << marker0_to_map_avg.getRotation()[1] <<"\n";
yaml_file << " z: " << marker0_to_map_avg.getRotation()[2] <<"\n";
yaml_file << " w: " << marker0_to_map_avg.getRotation()[3] <<"\n";
yaml_file << "marker1:\n"; // <!-- described in map coordinates -->
yaml_file << " position:\n";
yaml_file << " x: " << marker1_to_map_avg.getOrigin()[0] <<"\n";
yaml_file << " y: " << marker1_to_map_avg.getOrigin()[1] <<"\n";
yaml_file << " z: " << marker1_to_map_avg.getOrigin()[2] <<"\n";
yaml_file << " orientation:\n";
yaml_file << " x: " << marker1_to_map_avg.getRotation()[0] <<"\n";
yaml_file << " y: " << marker1_to_map_avg.getRotation()[1] <<"\n";
yaml_file << " z: " << marker1_to_map_avg.getRotation()[2] <<"\n";
yaml_file << " w: " << marker1_to_map_avg.getRotation()[3] <<"\n";
yaml_file << "marker2:\n"; // <!-- described in map coordinates -->
yaml_file << " position:\n";
yaml_file << " x: " << marker2_to_map_avg.getOrigin()[0] <<"\n";
yaml_file << " y: " << marker2_to_map_avg.getOrigin()[1] <<"\n";
yaml_file << " z: " << marker2_to_map_avg.getOrigin()[2] <<"\n";
yaml_file << " orientation:\n";
yaml_file << " x: " << marker2_to_map_avg.getRotation()[0] <<"\n";
yaml_file << " y: " << marker2_to_map_avg.getRotation()[1] <<"\n";
yaml_file << " z: " << marker2_to_map_avg.getRotation()[2] <<"\n";
yaml_file << " w: " << marker2_to_map_avg.getRotation()[3] <<" \n";
yaml_file.close();
while( ros::ok() ){
br.sendTransform(tf::StampedTransform(marker0_to_common_axis, ros::Time::now(), marker0_frame_id, "marker0_avg"));
br.sendTransform(tf::StampedTransform(marker1_to_common_axis, ros::Time::now(), marker1_frame_id, "marker1_avg"));
br.sendTransform(tf::StampedTransform(marker2_to_common_axis, ros::Time::now(), marker2_frame_id, "marker2_avg"));
br.sendTransform(tf::StampedTransform(map_to_common_axis0, ros::Time::now(), marker0_frame_id, "init_frame_0"));
br.sendTransform(tf::StampedTransform(map_to_common_axis1, ros::Time::now(), marker1_frame_id, "init_frame_1"));
br.sendTransform(tf::StampedTransform(map_to_common_axis2, ros::Time::now(), marker2_frame_id, "init_frame_2"));
br.sendTransform(tf::StampedTransform(map_to_common_axis_avg, ros::Time::now(), marker2_frame_id, "init_frame_avg"));
sleep_rate.sleep();
}
}
}
