//function travel around Eigen::Vector3s motionCampus(unsigned int ii, double& displacement_, double& rotation_) { if (ii <= 120){ displacement_ = 0.1; rotation_ = 0; } else if (ii <= 170) { displacement_ = 0.2; rotation_ = 1.8 * M_PI / 180; } else if (ii <= 220) { displacement_ = 0; rotation_ =-1.8 * M_PI / 180; } else if (ii <= 310) { displacement_ = 0.1; rotation_ = 0; } else if (ii <= 487) { displacement_ = 0.1; rotation_ =-1.0 * M_PI / 180; } else if (ii <= 600) { displacement_ = 0.2; rotation_ = 0; } else if (ii <= 700) { displacement_ = 0.1; rotation_ =-1.0 * M_PI / 180; } else if (ii <= 780) { displacement_ = 0; rotation_ =-1.0 * M_PI / 180; } else { displacement_ = 0.3; rotation_ = 0; } devicePose.moveForward(displacement_); devicePose.rt.setEuler(devicePose.rt.head() + rotation_, devicePose.rt.pitch(), devicePose.rt.roll()); // laser 1 laser1Pose.setPose(devicePose); laser1Pose.moveForward(laser_1_pose_(0)); // laser 2 laser2Pose.setPose(devicePose); laser2Pose.moveForward(laser_2_pose_(0)); laser2Pose.rt.setEuler(laser2Pose.rt.head() + laser_2_pose_(3), laser2Pose.rt.pitch(), laser2Pose.rt.roll()); devicePoses.push_back(devicePose); ground_truth_pose_ << devicePose.pt(0), devicePose.pt(1), devicePose.rt.head(); return ground_truth_pose_; }
void render(const FeatureBaseList& feature_list, int laser, const LandmarkBaseList& landmark_list, const Eigen::Vector3s& estimated_pose) { // detected corners //std::cout << " drawCorners: " << feature_list.size() << std::endl; std::vector<double> corner_vector; corner_vector.reserve(2*feature_list.size()); for (auto corner : feature_list) { //std::cout << " corner " << corner->id() << std::endl; corner_vector.push_back(corner->getMeasurement(0)); corner_vector.push_back(corner->getMeasurement(1)); } myRender->drawCorners(laser == 1 ? laser1Pose : laser2Pose, corner_vector); // landmarks //std::cout << " drawLandmarks: " << landmark_list.size() << std::endl; std::vector<double> landmark_vector; landmark_vector.reserve(3*landmark_list.size()); for (auto landmark : landmark_list) { Scalar* position_ptr = landmark->getPPtr()->getPtr(); landmark_vector.push_back(*position_ptr); //x landmark_vector.push_back(*(position_ptr + 1)); //y landmark_vector.push_back(0.2); //z } myRender->drawLandmarks(landmark_vector); // draw localization and sensors estimated_vehicle_pose.setPose(estimated_pose(0), estimated_pose(1), 0.2, estimated_pose(2), 0, 0); estimated_laser_1_pose.setPose(estimated_vehicle_pose); estimated_laser_1_pose.moveForward(laser_1_pose_(0)); estimated_laser_2_pose.setPose(estimated_vehicle_pose); estimated_laser_2_pose.moveForward(laser_2_pose_(0)); estimated_laser_2_pose.rt.setEuler(estimated_laser_2_pose.rt.head() + laser_2_pose_(3), estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll()); myRender->drawPoseAxisVector( { estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose }); //Set view point and render the scene //locate visualization view point, somewhere behind the device // viewPoint.setPose(devicePose); // viewPoint.rt.setEuler( viewPoint.rt.head(), viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() ); // viewPoint.moveForward(-5); myRender->setViewPoint(viewPoint); myRender->drawPoseAxis(devicePose); myRender->drawScan(laser == 1 ? laser1Pose : laser2Pose, laser == 1 ? scan1 : scan2, 180. * M_PI / 180., 90. * M_PI / 180.); //draw scan myRender->render(); }
FaramoticsRobot(int argc, char** argv, const Eigen::Vector4s& _laser_1_pose, const Eigen::Vector4s& _laser_2_pose) : modelFileName("/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj"), laser_1_pose_(_laser_1_pose), laser_2_pose_(_laser_2_pose) { devicePose.setPose(2, 8, 0.2, 0, 0, 0); viewPoint.setPose(devicePose); viewPoint.moveForward(10); viewPoint.rt.setEuler(viewPoint.rt.head() + M_PI / 2, viewPoint.rt.pitch() + 30. * M_PI / 180., viewPoint.rt.roll()); viewPoint.moveForward(-15); //glut initialization faramotics::initGLUT(argc, argv); //create a viewer for the 3D model and scan points myRender = new CdynamicSceneRender(1200, 700, 90 * M_PI / 180, 90 * 700.0 * M_PI / (1200.0 * 180.0), 0.2, 100); myRender->loadAssimpModel(modelFileName, true); //with wireframe //create scanner and load 3D model myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG); //HOKUYO_UTM30LX_180DEG or LEUZE_RS4 myScanner->loadAssimpModel(modelFileName); }
//function travel around void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double& rotation_) { if (ii <= 120) { displacement_ = 0.1; rotation_ = 0; } else if ((ii > 120) && (ii <= 170)) { displacement_ = 0.2; rotation_ = 1.8 * M_PI / 180; } else if ((ii > 170) && (ii <= 220)) { displacement_ = 0; rotation_ = -1.8 * M_PI / 180; } else if ((ii > 220) && (ii <= 310)) { displacement_ = 0.1; rotation_ = 0; } else if ((ii > 310) && (ii <= 487)) { displacement_ = 0.1; rotation_ = -1. * M_PI / 180; } else if ((ii > 487) && (ii <= 600)) { displacement_ = 0.2; rotation_ = 0; } else if ((ii > 600) && (ii <= 700)) { displacement_ = 0.1; rotation_ = -1. * M_PI / 180; } else if ((ii > 700) && (ii <= 780)) { displacement_ = 0; rotation_ = -1. * M_PI / 180; } else { displacement_ = 0.3; rotation_ = 0.0 * M_PI / 180; } pose.moveForward(displacement_); pose.rt.setEuler(pose.rt.head() + rotation_, pose.rt.pitch(), pose.rt.roll()); }