GridRenderEngine::GridRenderEngine(double _xsize, double _ysize, double _zsize, double _xdist, double _ydist, double _zdist)
{
mSize << _xsize, _ysize, _zsize;
mCellSize << _xdist, _ydist, _zdist;
mName = "Grid";
updatePose();
}
/* updatePoseRecursive */
void ArticulatedObject::updatePoseRecursive(const Eigen::Matrix4f &_pose)
{
updatePose(_pose);
for (unsigned i=0; i<subparts.size(); i++) {
updatePoseRecursive(pose, *parts[subparts[i]], parts);
}
}
GridRenderEngine::GridRenderEngine()
{
mSize << 1000, 1000, 100;
mCellSize << 100, 100, 100;
mName = "Grid";
updatePose();
}
/**************************************
* Definition: Turns the robot as close to the specified theta
* as possible. When theta is within the specified
* theta error threshold, it returns.
*
* Parameters: floats specifying theta goal and theta error limit
**************************************/
void Robot::turnTo(float thetaGoal, float thetaErrorLimit) {
float theta;
float thetaError;
float turnGain;
printf("adjusting theta\n");
do {
updatePose(false);
theta = _northStar->getTheta();
thetaError = thetaGoal - theta;
thetaError = Util::normalizeThetaError(thetaError);
turnGain = _turnPID->updatePID(thetaError);
if (thetaError < -thetaErrorLimit) {
int turnSpeed = (int)(10 - 9 * turnGain);
turnSpeed = Util::capSpeed(turnSpeed, 10);
turnRight(turnSpeed);
}
else if(thetaError > thetaErrorLimit){
int turnSpeed = (int)(10 - 9 * turnGain);
turnSpeed = Util::capSpeed(turnSpeed, 10);
turnLeft(turnSpeed);
}
}
while (fabs(thetaError) > thetaErrorLimit);
printf("theta acceptable\n");
}
/**************************************
* Definition: Fills sensor filters entirely
**************************************/
void Robot::prefillData() {
printf("prefilling data\n");
for (int i = 0; i < MAX_FILTER_TAPS; i++){
updatePose(true);
}
printf("sufficient data collected\n");
}
void RobotNode::setJointValue(float q)
{
RobotPtr r = getRobot();
VR_ASSERT(r);
WriteLockPtr lock = r->getWriteLock();
setJointValueNoUpdate(q);
updatePose();
}
GroundRenderEngine::GroundRenderEngine(double _xsize, double _zsize, double _xdist, double _zdist)
: mXsize(_xsize), mZsize(_zsize), mXdist(_xdist), mZdist(_zdist),
mPrimaryRed(0.2), mPrimaryGreen(0.2), mPrimaryBlue(0.2),
mSecondaryRed(0.5), mSecondaryGreen(0.5), mSecondaryBlue(0.5)
{
mName = "ground plate";
updatePose();
}
/**************************************
* Definition: Attempts to move the robot to the specified location
* in the global coord system, disregarding cells. If
* theta error is exceeded, the method returns theta error.
*
* Parameters: floats specifying x and y in global system, and
* a float specifying the theta error limit
**************************************/
float Robot::moveToUntil(float x, float y, float distErrorLimit, float thetaErrorLimit) {
float yError;
float xError;
float thetaDesired;
float thetaError;
float distError;
float moveGain;
printf("heading toward (%f, %f)\n", x, y);
do {
updatePose(true);
yError = y - _pose->getY();
xError = x - _pose->getX();
switch (_heading) {
case DIR_NORTH:
thetaDesired = DEGREE_90;
distError = fabs(y - _pose->getY());
break;
case DIR_SOUTH:
thetaDesired = DEGREE_270;
distError = fabs(y - _pose->getY());
break;
case DIR_EAST:
thetaDesired = DEGREE_0;
distError = fabs(x - _pose->getX());
break;
case DIR_WEST:
thetaDesired = DEGREE_180;
distError = fabs(x - _pose->getX());
break;
}
thetaError = thetaDesired - _northStar->getTheta();
thetaError = Util::normalizeThetaError(thetaError);
moveGain = _movePID->updatePID(distError);
_turnPID->updatePID(thetaError);
if (fabs(thetaError) > thetaErrorLimit) {
printf("theta error of %f too great\n", thetaError);
return thetaError;
}
int moveSpeed = (int)(10 - 9 * moveGain);
moveSpeed = Util::capSpeed(moveSpeed, 10);
moveForward(moveSpeed);
}
while (distError > distErrorLimit);
return 0; // no error when we've finished
}
void MetaBlock::updatePose(mscene *current_scene,
const geometry_msgs::Pose &pose)
{
updatePose(pose);
//pub_obj_moveit->publish(collObj_);
std::vector<moveit_msgs::CollisionObject> coll_objects;
coll_objects.push_back(collObj_);
current_scene->addCollisionObjects(coll_objects);
sleep(0.5);
}
void ServerDriver::openvr_poseUpdate(uint32_t unWhichDevice, vr::DriverPose_t & newPose, int64_t timestamp) {
auto devicePtr = this->m_openvrIdToVirtualDeviceMap[unWhichDevice];
auto now = std::chrono::duration_cast <std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
auto diff = 0.0;
if (timestamp < now) {
diff = ((double)now - timestamp) / 1000.0;
}
if (devicePtr) {
devicePtr->updatePose(newPose, -diff);
} else {
if (_openvrIdToDeviceManipulationHandleMap[unWhichDevice] && _openvrIdToDeviceManipulationHandleMap[unWhichDevice]->isValid()) {
newPose.poseTimeOffset -= diff;
_openvrIdToDeviceManipulationHandleMap[unWhichDevice]->ll_sendPoseUpdate(newPose);
}
}
}
int PSMoveTemplateController::updateSuccess(MoveServerPacket *move_server_packet)
{
//ROS_ERROR("updateSuccess: %f, %f, %f, %d", move_server_packet->state[0].pos[0], move_server_packet->state[0].pos[1], move_server_packet->state[0].pos[2], move_server_packet->state[0].pad.analog_trigger);
//Only update if trigger is pressed and there is an object selected
if(move_server_packet->state[0].pad.analog_trigger > 0 && selected_object_topic_ != "")
{
if(last_analog_trigger_ == 0)
first_time_ = true;
// create an interactive marker update message using that string
vigir_ocs_msgs::OCSInteractiveMarkerUpdate cmd;
//Update current pose
geometry_msgs::PoseStamped p;
p = updatePose(object_pose_, move_server_packet, /*(float)move_server_packet->state[0].pad.analog_trigger/255.0f*/1.0f);
//Add to the template_update
cmd.client_id = ros::this_node::getName();
cmd.topic = selected_object_topic_;
cmd.pose = p;
cmd.pose.header.frame_id = "/world";
cmd.pose.header.stamp = ros::Time::now();
cmd.update_mode = vigir_ocs_msgs::OCSInteractiveMarkerUpdate::SET_POSE;
//Publish the new pose
interactive_marker_update_pub_.publish(cmd);
cout << "object pose updated and published" << "\n";
}
// save last trigger value
last_analog_trigger_ = move_server_packet->state[0].pad.analog_trigger;
//Save position and orientation
old_move_position_.setX(move_server_packet->state[0].handle_pos[0]);
old_move_position_.setY(move_server_packet->state[0].handle_pos[1]);
old_move_position_.setZ(move_server_packet->state[0].handle_pos[2]);
old_move_orientation_.setX(move_server_packet->state[0].quat[0]);
old_move_orientation_.setY(move_server_packet->state[0].quat[1]);
old_move_orientation_.setZ(move_server_packet->state[0].quat[2]);
old_move_orientation_.setScalar(move_server_packet->state[0].quat[3]);
return 0;
}
void PictogramObject::update(float wall_dt, float ros_dt)
{
if (text_.empty()) {
// not yet setted
return;
}
// update position and orientation
if (!context_) {
return;
}
updatePose(wall_dt);
if (!need_to_update_) {
return;
}
need_to_update_ = false;
ScopedPixelBuffer buffer = texture_object_->getBuffer();
QColor transparent(255, 255, 255, 0);
QImage Hud = buffer.getQImage(128, 128, transparent); // should change according to size
QPainter painter( &Hud );
painter.setRenderHint(QPainter::Antialiasing, true);
QColor foreground = rviz::ogreToQt(color_);
painter.setPen(QPen(foreground, 5, Qt::SolidLine));
if (isCharacterSupported(text_)) {
QFont font = getFont(text_);
QString pictogram_text = lookupPictogramText(text_);
if (isEntypo(text_)) {
font.setPointSize(100);
}
else if (isFontAwesome(text_)) {
font.setPointSize(45);
}
painter.setFont(font);
painter.drawText(0, 0, 128, 128,
Qt::AlignHCenter | Qt::AlignVCenter,
pictogram_text);
painter.end();
}
else {
ROS_WARN("%s is not supported", text_.c_str());
}
}
// Class implementation of a spinOnce() type function.
// Checks for new data, if both are new then update pose.
void ambcap_pros::spinOnce() {
if ( newDataRShank && newDataRThigh ) {
updatePose();
}
}
void KalmanFilter::updateMeasurement(double measurement, double variance,
const Eigen::Matrix<double, 1, 2>& C, double timestamp) {
updatePose(timestamp);
updateState(measurement, variance, C, predictState(timestamp));
lastMeasurementTimestamp_ = timestamp;
}
/**************************************
* Definition: Strafes the robot until it is considered centered
* between two squares in a corridor
**************************************/
void Robot::center() {
moveHead(RI_HEAD_MIDDLE);
int turnAttempts = 0;
Camera::prevTagState = -1;
while (true) {
bool turn = false;
float centerError = _camera->centerError(COLOR_PINK, &turn);
if (turn) {
if (_centerTurn(centerError)) {
break;
}
turnAttempts++;
}
else {
if (_centerStrafe(centerError)) {
break;
}
}
if (turnAttempts > 2) {
moveHead(RI_HEAD_DOWN);
// make sure we are close to our desired heading, in case we didn't move correctly
updatePose(false);
float thetaHeading;
switch (_heading) {
case DIR_NORTH:
thetaHeading = DEGREE_90;
break;
case DIR_SOUTH:
thetaHeading = DEGREE_270;
break;
case DIR_EAST:
thetaHeading = DEGREE_0;
break;
case DIR_WEST:
thetaHeading = DEGREE_180;
break;
}
float theta = _pose->getTheta();
float thetaError = thetaHeading - theta;
thetaError = Util::normalizeThetaError(thetaError);
if (fabs(thetaError) > DEGREE_45) {
// if we turned beyond 45 degrees from our
// heading, this was a mistake. let's turn
// back just enough so we're 45 degrees from
// our heading.
float thetaGoal = Util::normalizeTheta(theta + (DEGREE_45 + thetaError));
turnTo(thetaGoal, MAX_THETA_ERROR);
}
turnAttempts = 0;
moveHead(RI_HEAD_MIDDLE);
}
}
moveHead(RI_HEAD_DOWN);
_centerTurnPID->flushPID();
_centerStrafePID->flushPID();
}
void Mechanism::stepWorld(){
pdController(); // apply pd control
dynamicsWorld_->stepSimulation(1/60.f,10); // step simulation
updatePose(); // update rbt transform and pose_
}
void DebugAttachmentSystem::onFrameUpdate(const UpdateFrame & updateFrame)
{
auto & imGuiSystem = world().systemRef<ImGuiSystem>();
if (!imGuiSystem.showView("Debug Attachments"))
{
m_visibleNode->setVisible(false);
m_obscuredNode->setVisible(false);
return;
}
m_visibleNode->setVisible(true);
m_obscuredNode->setVisible(false);
size_t currentAttachmentIndex = 0;
for (const auto & entityEntry : m_entities)
{
if (!entityEntry.active) continue;
const auto entity = world().entityById(entityEntry.id);
const auto & equipment = entity.component<Equipment>();
for (const auto & attachment : equipment.attachments())
{
auto transform = Transform3D::fromPose(attachment->worldPose());
transform.setScale(
entity.component<Transform3DComponent>().transform().scale());
updateSphere(
m_visibleNode->sphere(currentAttachmentIndex),
attachment,
transform);
updateSphere(
m_obscuredNode->sphere(currentAttachmentIndex),
attachment,
transform);
updatePose(
m_visibleNode->pose(currentAttachmentIndex),
attachment,
transform);
updatePose(
m_obscuredNode->pose(currentAttachmentIndex),
attachment,
transform);
currentAttachmentIndex++;
}
}
m_numAllocatedPrimitives =
std::max(currentAttachmentIndex, m_numAllocatedPrimitives);
for (; currentAttachmentIndex < m_numAllocatedPrimitives;
currentAttachmentIndex++)
{
m_visibleNode->pose(currentAttachmentIndex).setVisible(false);
m_obscuredNode->pose(currentAttachmentIndex).setVisible(false);
m_visibleNode->sphere(currentAttachmentIndex).setVisible(false);
m_obscuredNode->sphere(currentAttachmentIndex).setVisible(false);
}
}
void QArClient::updateNumbersReceived(ArRobotInfo robotInfo)
{
emit updatePose(robotInfo);
}
void QArClient::updateStringsReceived(ArRobotInfo robotInfo)
{
emit updatePose(robotInfo);
}
//-----------------------------------------------------------------------------
//
physx::PxTransform FollowPoseComponent::pose()
{
_lastPose = updatePose();
return _lastPose;
}
//=============================================================================
// CLASS FollowPoseComponent
//=============================================================================
//-----------------------------------------------------------------------------
//
void FollowPoseComponent::follow( const GameObjectRef &iGameObject)
{
_followedGO = iGameObject;
updatePose();
}
Robot::Robot(std::string address, int id) {
// store the robot's name as an int to be used later
_name = Util::nameFrom(address);
// initialize movement variables
_numCellsTraveled = 0;
_turnDirection = 0;
_movingForward = true;
_speed = 0;
_heading = DIR_NORTH;
setFailLimit(MAX_UPDATE_FAILS);
_robotInterface = new RobotInterface(address, id);
printf("robot interface loaded\n");
// initialize camera
_camera = new Camera(_robotInterface);
// initialize position sensors
_wheelEncoders = new WheelEncoders(this);
_northStar = new NorthStar(this);
// initialize global pose
_pose = new Pose(0.0, 0.0, 0.0);
// bind _pose to the kalman filter
_kalmanFilter = new KalmanFilter(_pose);
_kalmanFilter->setUncertainty(PROC_X_UNCERTAIN,
PROC_Y_UNCERTAIN,
PROC_THETA_UNCERTAIN,
NS_X_UNCERTAIN,
NS_Y_UNCERTAIN,
NS_THETA_UNCERTAIN,
WE_X_UNCERTAIN,
WE_Y_UNCERTAIN,
WE_THETA_UNCERTAIN);
printf("kalman filter initialized\n");
PIDConstants movePIDConstants = {PID_MOVE_KP, PID_MOVE_KI, PID_MOVE_KD};
PIDConstants turnPIDConstants = {PID_TURN_KP, PID_TURN_KI, PID_TURN_KD};
_movePID = new PID(&movePIDConstants, MIN_MOVE_ERROR, MAX_MOVE_ERROR);
_turnPID = new PID(&turnPIDConstants, MIN_TURN_ERROR, MAX_TURN_ERROR);
printf("pid controllers initialized\n");
// Put robot head down for NorthStar use
moveHead(RI_HEAD_DOWN);
printf("collecting initial data\n");
// fill our sensors with data
prefillData();
// base the wheel encoder pose off north star to start,
// since we might start anywhere in the global system)
_wheelEncoders->resetPose(_northStar->getPose());
// now update our pose again so our global pose isn't
// some funky value (since it's based off we and ns)
updatePose(true);
// load the map once we've found our position in the global
// system, so we can know what cell we started at
int startingX;
int startingY;
if (_pose->getX() > 150) {
startingX = 0;
startingY = 2;
}
else {
startingX = 6;
startingY = 2;
}
_map = new Map(_robotInterface, startingX, startingY);
_mapStrategy = new MapStrategy(_map);
printf("map loaded\n");
}
/**************************************
* Definition: Moves the robot in the specified direction
* the specified number of cells (65x65 area)
*
* Parameters: ints specifying direction and number of cells
**************************************/
void Robot::move(int direction, int numCells) {
_heading = direction;
int cellsTraveled = 0;
while (cellsTraveled < numCells) {
if(_numCellsTraveled != 0) {
// make sure we're facing the right direction first
if (nsThetaReliable()) {
turn(direction);
}
// center ourselves between the walls
center();
// turn once more to fix any odd angling
if (nsThetaReliable()) {
turn(direction);
}
}
// count how many squares we see down the hall
moveHead(RI_HEAD_MIDDLE);
float leftSquareCount = _camera->avgSquareCount(COLOR_PINK, IMAGE_LEFT);
float rightSquareCount = _camera->avgSquareCount(COLOR_PINK, IMAGE_RIGHT);
moveHead(RI_HEAD_DOWN);
if (leftSquareCount > 3.0) {
leftSquareCount = 3.0;
}
if (rightSquareCount > 3.0) {
rightSquareCount = 3.0;
}
// update our pose estimates now, ignoring
// wheel encoders and setting them to be north star's
updatePose(false);
_wheelEncoders->resetPose(_northStar->getPose());
// finally, update our pose one more time so kalman isn't wonky
updatePose(true);
// based on the direction, move in the global coord system
float goalX = _pose->getX();
float goalY = _pose->getY();
float distErrorLimit = MAX_DIST_ERROR;
switch (direction) {
case DIR_NORTH:
goalY += CELL_SIZE;
break;
case DIR_SOUTH:
goalY -= CELL_SIZE;
break;
case DIR_EAST:
goalX += CELL_SIZE;
if (_map->getCurrentCell()->x == 0 ||
_map->getCurrentCell()->x == 1) {
goalX -= 30.0;
}
break;
case DIR_WEST:
goalX -= CELL_SIZE;
if (_map->getCurrentCell()->x == 0 ||
_map->getCurrentCell()->x == 1) {
goalX += 30.0;
}
break;
}
moveTo(goalX, goalY, distErrorLimit);
// make sure we stop once we're there
stop();
moveHead(RI_HEAD_MIDDLE);
float newLeftSquareCount = _camera->avgSquareCount(COLOR_PINK, IMAGE_LEFT);
float newRightSquareCount = _camera->avgSquareCount(COLOR_PINK, IMAGE_RIGHT);
if (newLeftSquareCount > 3.0) {
newLeftSquareCount = 3.0;
}
if (newRightSquareCount > 3.0) {
newRightSquareCount = 3.0;
}
int i = 0;
while ((newLeftSquareCount + 1.0 < leftSquareCount ||
newRightSquareCount + 1.0 < rightSquareCount) &&
i < 5) {
moveBackward(10);
_robotInterface->reset_state();
newLeftSquareCount = _camera->avgSquareCount(COLOR_PINK, IMAGE_LEFT);
newRightSquareCount = _camera->avgSquareCount(COLOR_PINK, IMAGE_RIGHT);
if (newLeftSquareCount + 1.0 < leftSquareCount &&
newRightSquareCount + 1.0 < rightSquareCount) {
i++;
}
i++;
}
moveHead(RI_HEAD_DOWN);
// we made it!
cellsTraveled++;
printf("Made it to cell %d\n", cellsTraveled);
}
}
GridRenderEngine::GridRenderEngine(const Vector3d &_size, const Vector3d &_cellSize)
: mSize(_size), mCellSize(_cellSize)
{
mName = "Grid";
updatePose();
}