// 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); }