// Pre-condition: Secondary map metadata has already been published
void MapManager::loadOrigin()
{
x_man_editor->setValue(secondary_map.info.origin.position.x);
y_man_editor->setValue(secondary_map.info.origin.position.y);
float yaw = getYawFromQuat(secondary_map.info.origin.orientation.x, secondary_map.info.origin.orientation.y, secondary_map.info.origin.orientation.z, secondary_map.info.origin.orientation.w);
theta_man_editor->setValue(yaw);
}
void DirectOrthoNodelet::getStartNodeletCallback(const geometry_msgs::PoseWithCovarianceStamped event)
{
geometry_msgs::Pose2DPtr msg(new geometry_msgs::Pose2D);
msg->x = event.pose.pose.position.x;
msg->y = event.pose.pose.position.y;
msg->theta = getYawFromQuat(event.pose.pose.orientation);
setStartNodeletPub.publish(msg);
}
void MapManager::sec_comm_pose_cb(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{
x_auto_sec_editor->setValue(msg->pose.pose.position.x);
y_auto_sec_editor->setValue(msg->pose.pose.position.y);
float yaw = getYawFromQuat(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
theta_auto_sec_editor->setValue(yaw);
}
void MapManager::autoAlignMaps()
{
float initial_x = secondary_map.info.origin.position.x;
float initial_y = secondary_map.info.origin.position.y;
// pre-compute theta, i.e. the angular difference between the common poses
float delta_theta = theta_auto_pri_editor->value() - theta_auto_sec_editor->value();
float theta = delta_theta + getYawFromQuat(secondary_map.info.origin.orientation.x, secondary_map.info.origin.orientation.y, secondary_map.info.origin.orientation.z, secondary_map.info.origin.orientation.w);
tf::Quaternion quat;
quat.setRPY(0.0, 0.0, theta);
// secondary map - common pose after rotation
float prev_sec_pose_x = x_auto_sec_editor->value() - initial_x;
float prev_sec_pose_y = y_auto_sec_editor->value() - initial_y;
float rotated_sec_pose_x = initial_x + (prev_sec_pose_x * cosf(delta_theta) - prev_sec_pose_y * sinf(delta_theta));
float rotated_sec_pose_y = initial_y + (prev_sec_pose_x * sinf(delta_theta) + prev_sec_pose_y * cosf(delta_theta));
float delta_x = x_auto_pri_editor->value() - rotated_sec_pose_x;
float delta_y = y_auto_pri_editor->value() - rotated_sec_pose_y;
map_store::SetOrigin setOriginSrv;
setOriginSrv.request.map_id = getMapId(secondary_map_combo->currentText().toStdString());
setOriginSrv.request.pos_x = initial_x + delta_x;
setOriginSrv.request.pos_y = initial_y + delta_y;
setOriginSrv.request.rot_x = quat.x();
setOriginSrv.request.rot_y = quat.y();
setOriginSrv.request.rot_z = quat.z();
setOriginSrv.request.rot_w = quat.w();
if (!sec_set_origin_client.call(setOriginSrv)) {
ROS_ERROR("SERVICE CALL FAILED: Could not set the desired origin - theta offset stage failed");
return;
}
setMap(setOriginSrv.request.map_id, sec_set_map_client);
}
