void MumbleManager::setMap(const std::string &mapName) { if (!mLinkedMem) return; // Context should be equal for players which should be able to hear each // other positional and differ for those who shouldn't // (e.g. it could contain the server+port and team) setMapBase(getMapId(mapName)); setAction(0); /* update y coordinate */ }
void MapManager::setSecondaryMapOrigin() { tf::Quaternion quat; quat.setRPY(0.0, 0.0, theta_man_editor->value()); map_store::SetOrigin setOriginSrv; setOriginSrv.request.map_id = getMapId(secondary_map_combo->currentText().toStdString()); setOriginSrv.request.pos_x = x_man_editor->value(); setOriginSrv.request.pos_y = y_man_editor->value(); 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 specified origin"); return; } setMap(setOriginSrv.request.map_id, sec_set_map_client); }
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); }
void MapManager::setSecondaryMap() { std::string map_id = getMapId(secondary_map_combo->currentText().toStdString()); setMap(map_id, sec_set_map_client); }
void MapManager::setPrimaryMap() { std::string map_id = getMapId(primary_map_combo->currentText().toStdString()); setMap(map_id, pri_set_map_client); }