void drive(const int x, const int y, const int phi )
{
rec::core_lt::Timer timer;
timer.start();
//while( com.isConnected()
// && false == bumper.value()
// && timer.msecsElapsed() < 10000 )
while( com.isConnected()
&& false == bumper.value()
&& odometry.x() < x )
{
omniDrive.setVelocity( 70, 0, 0 );
com.waitForUpdate();
}
while( com.isConnected()
&& false == bumper.value()
&& odometry.y() < y )
{
omniDrive.setVelocity( 0, 70, 0 );
com.waitForUpdate();
}
while( com.isConnected()
&& false == bumper.value()
&& odometry.phi() < phi )
{
omniDrive.setVelocity( 0, 0, 30 );
com.waitForUpdate();
}
}
void drive(const int x, const int y, const int phi )
{
const int vel=90;
rec::core_lt::Timer timer;
timer.start();
//while( com.isConnected()
// && false == bumper.value()
// && timer.msecsElapsed() < 10000 )
double M=sqrt(mysqr(odometry.x()-x)+mysqr(odometry.y()-y));
while( com.isConnected()
&& false == bumper.value()
&& M>30 )
{
M=sqrt(mysqr(odometry.x()-x)+mysqr(odometry.y()-y));
//std::cout<<M<<std::endl;
omniDrive.setVelocity( vel*((double)(x-odometry.x()))/M, vel*((double)(y-odometry.y()))/M, 0 );
com.waitForUpdate();
}
/*
while( com.isConnected()
&& false == bumper.value()
&& odometry.x() < x )
{
omniDrive.setVelocity( 70, 0, 0 );
com.waitForUpdate();
}
while( com.isConnected()
&& false == bumper.value()
&& odometry.y() < y )
{
omniDrive.setVelocity( 0, 70, 0 );
com.waitForUpdate();
}*/
while( com.isConnected()
&& false == bumper.value()
&& phi>0
&& (abs(odometry.phi()-phi)>5) )
{
omniDrive.setVelocity( 0, 0, +30 );
com.waitForUpdate();
}
while( com.isConnected()
&& false == bumper.value()
&& phi<0
&& (abs(odometry.phi()-phi)>5) )
{
omniDrive.setVelocity( 0, 0, -30 );
com.waitForUpdate();
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "odometryNode");
Odometry controller;
controller.init(argc, argv);
GCRobotics::Pose_msg data;
tf::TransformBroadcaster br;
tf::Transform transform;
tf::Quaternion q;
ros::Rate r(10); // 10 hz
while (ros::ok())
{
data.x = controller.X;
data.y = controller.Y;
data.z = 0;
data.heading = controller.heading;
controller.pub.publish(data);
transform.setOrigin( // Set global position to send to tf
tf::Vector3(
controller.X*sin(controller.heading) + controller.Y*cos(controller.heading) ,
controller.X*cos(controller.heading) - controller.Y*sin(controller.heading) ,
0.0 ) );
q.setRPY(0,0,controller.heading); // Convert heading calculation into quaternion to give to tf.
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom", "base_link")); // Send the transform
ros::spinOnce();
r.sleep();
}
}
void
odometryupdate(const nubot_common::OdoInfo & _motorinfo_msg)
{
static std::vector<double> motor_data(nubot::MOTOR_NUMS_CONST,0);
static ros::Time time_before = _motorinfo_msg.header.stamp;
ros::Time time_update = _motorinfo_msg.header.stamp;
motor_data[0]=(double)_motorinfo_msg.Vx;
motor_data[1]=(double)_motorinfo_msg.Vy;
motor_data[2]=(double)_motorinfo_msg.w;
is_power_off_ = _motorinfo_msg.PowerState;
is_robot_stuck_ = _motorinfo_msg.RobotStuck;
ros::Duration duration= time_update-time_before;
time_before = time_update ;
double secs=duration.toSec();
odometry_->process(motor_data,secs);
}
void init( const std::string& hostname)
{
// Initialize the actors
motor1.setMotorNumber( 0 );
motor2.setMotorNumber( 1 );
motor3.setMotorNumber( 2 );
// Connect
std::cout << "Connecting to "<< hostname << " ... " << std::endl;
com.setAddress( hostname.c_str() );
com.connect();
odometry.set(0, 0, 0);
std::cout << std::endl << "Connected" << std::endl;
}
/// Display stuff
void publishStuff(bool all = true){
if (pubMarker.getNumSubscribers()>0){
// Publish Landmarks
pubMarker.publish(odometry.getMapMarkers());
pubMarker.publish(odometry.getMapObservations());
// Publish Track (ie all path and poses estimated)
pubMarker.publish(odometry.getTrackLineMarker());
}
if (pubTrack.getNumSubscribers()>0){
pubTrack.publish(odometry.getTrackPoseMarker());
}
if (all){
// Publish TF for each KF
ROS_INFO("NOD = Publishing TF for each KF");
const Frame::Ptrs& kfs = odometry.getKeyFrames();
//const Frame::Ptr kf = kfs[0];
for(uint i=0; i<kfs.size(); ++i){
pubTF.sendTransform(kfs[i]->getStampedTransform());
}
}
// Publish TF for current frame
const Frame::Ptr f = odometry.getLastFrame();
if (f->poseEstimated()){
pubTF.sendTransform(f->getStampedTransform());
pubTF.sendTransform(f->getStampedTransform(true));
}
}
void
process()
{
if(!colorsegment_->Segment(imginfo_->yuv_image_))
return;
//get the colors of the scan_points
scanpts_->process();
//detect the white points
whites_->process();
if(whites_->img_white_.size()<=0)
{
// is_restart_=true;
ROS_WARN("don't detect the white points");
return ;
}
if(!is_power_off_)
is_restart_=true;
if(is_restart_)
{
is_restart_=false;
robot.isglobal_=true;
}
//localization
if(robot.isglobal_)
{
ROS_INFO("start global localization");
robot.isglobal_=glocation_->process(whites_->weights_,whites_->robot_white_,robot.visual_location_,robot.angle_);
}
else
{
DPoint delta_loc=odometry_->getWorldLocaton();
Angle delta_ang=odometry_->getDeltaAngle();
odometry_->clear(robot.angle_);
robot.realvtrans_ = odometry_->getRealVelocity();
robot.worldvtrans_ = odometry_->getWorldVelocity();
robot.angular_velocity_ = odometry_->getAngularVelocity();
location_->process(whites_->weights_,whites_->robot_white_,robot.visual_location_,robot.angle_,delta_loc,delta_ang);
}
PPoint correct_pt = PPoint(tranfer_->get_offset().angle(),tranfer_->get_offset().norm());
tranfer_->calculateWorldCoordinates(correct_pt,robot.visual_location_,robot.angle_,robot.final_location_);
obstacles_->process(colorsegment_->segment_result_,robot.final_location_,robot.angle_);
ball_info_.pos_known=ball_finder_->Process(colorsegment_->segment_result_,robot.final_location_,robot.angle_);
if(is_show_whites_ || is_show_obstacles_ || is_show_ball_ )
{
if(field_image_.empty())
ROS_INFO("Field.bmp is empty");
cv::Mat image = field_image_.clone();
cv::Mat orgnal = imginfo_->getBGRImage().clone();
static double length = 1920;
static double width = 1314;
if(WIDTH_RATIO<1)
{
length = 1920;
width = 882;
}
if(is_show_scan_points)
scanpts_->showScanPoints();
if(is_show_obstacles_)
{
obstacles_->showObstacles(orgnal);
if(!field_image_.empty())
obstacles_->showObstacles(image,robot.final_location_,robot.angle_,length,width);
}
if(is_show_whites_)
{
whites_->showWhitePoints(orgnal);
if(!field_image_.empty())
whites_->showWhitePoints(image,robot.final_location_,robot.angle_,length,width);
}
if(is_show_ball_)
{
ball_finder_->showBall(orgnal);
if(!field_image_.empty())
ball_finder_->showBall(image,robot.final_location_,robot.angle_,length,width);
}
}
}
/*-----------------------------------------------------------------------------
* Motion thread tick function (copy from runswift MotionAdapter.cpp)
This would be like processFrame() function as in our code
*---------------------------------------------------------------------------*/
void RSWalkModule2014::processFrame() {
// If we are changing commands we need to reset generators
if(walk_request_->motion_ != prev_command && !walk_request_->perform_kick_){
if(DEBUG_OUTPUT) cout << "RESETTING GENERATORS! " << WalkRequestBlock::getName(walk_request_->motion_) << " != " << WalkRequestBlock::getName(prev_command) << "\n";
standing = true;
}
// Setting walk kick parameters in bodyModel
bodyModel.walkKick = walk_request_->perform_kick_;
bodyModel.walkKickLeftLeg = walk_request_->kick_with_left_;
bodyModel.walkKickHeading = walk_request_->kick_heading_;
// 1. Need odometry to send to WalkGenerator. makeJoints updates odometry for localization
Odometry odo = Odometry(odometry_->displacement.translation.x, odometry_->displacement.translation.y, odometry_->displacement.rotation);
Odometry prev = Odometry(odo); // Another copy for after makeJoints call
// 2. Convert our request to runswift request
// 2.1 Head
ActionCommand::Head head; // Use default head. Not using HeadGenerator
// 2.2 Body
ActionCommand::Body body;
if (walk_request_->motion_ == WalkRequestBlock::STAND)
{
if(DEBUG_OUTPUT) cout << "Requested: STANDUP\n";
body.actionType = ActionCommand::Body::STAND;
body.forward = 0;
body.left = 0;
body.turn = 0;
body.isFast = false;
body.bend = 0;
}
else if (walk_request_->motion_ == WalkRequestBlock::STAND_STRAIGHT)
{
if (DEBUG_OUTPUT) cout << "Requested: STANDSTRAIGHT\n";
body.actionType = ActionCommand::Body::STAND_STRAIGHT;
body.forward = 0;
body.left = 0;
body.turn = 0;
body.isFast = false;
body.bend = 0;
}
else if (walk_request_->motion_ == WalkRequestBlock::WALK)
{
// if (walk_request_->walk_to_target_){
// if(DEBUG_OUTPUT) cout << "Requested: Target\n";
// processTargetModeWalk(body);
// } else{
if(DEBUG_OUTPUT) cout << "Requested: WALK\n";
body.actionType = ActionCommand::Body::WALK;
body.forward = walk_request_->speed_.translation.x * walk_params_->bh_params_.rsSpeedMax.translation.x;
body.left = walk_request_->speed_.translation.y * walk_params_->bh_params_.rsSpeedMax.translation.y;
body.turn = walk_request_->speed_.rotation * walk_params_->bh_params_.rsSpeedMax.rotation;
body.bend = 1;
// This was a hack for odometry turning.
//if (walk_request_->speed_.translation.x > 0.5) //TODO threshold
//body.turn += walk_params_->bh_params_.rs_turn_angle_offset;
body.isFast = false;
// }
}
else if (walk_request_->motion_ == WalkRequestBlock::NONE || walk_request_->motion_ == WalkRequestBlock::WAIT)
{
if(DEBUG_OUTPUT) cout << "Requested: " << WalkRequestBlock::getName(walk_request_->motion_) << "\n";
body.actionType = ActionCommand::Body::ActionType::NONE;
body.forward = 0;
body.bend = 1;
body.left = 0;
body.turn = 0;
body.isFast = false;
}
else{
if(DEBUG_OUTPUT) cout << "Other Request: " << walk_request_->motion_ << endl;
body.actionType = ActionCommand::Body::ActionType::STAND;
body.forward = 0;
body.left = 0;
body.turn = 0;
body.isFast = false;
}
/* if (walk_request_->walk_to_target_){//perform_kick_){// && body.actionType != ActionCommand::Body::ActionType::NONE){
if(DEBUG_OUTPUT) cout << "RS Kick request" << endl;
body.actionType = ActionCommand::Body::ActionType::KICK;
body.isFast = true;
body.forward = 0;
body.left = 0;
body.turn = 0;//walk_request_->kick_heading_;
body.bend = 1;
body.speed = 0.5;
body.power = 0.5;
if (walk_request_->kick_with_left_)
body.foot = ActionCommand::Body::LEFT;
else
body.foot = ActionCommand::Body::RIGHT;
}
*/
if(!body_model_->feet_on_ground_ && body.actionType != ActionCommand::Body::ActionType::NONE)
{
// Need something here so robot stops when picked up
body.actionType = ActionCommand::Body::ActionType::REF_PICKUP;
}
// 2.3 LEDs and Sonar
ActionCommand::LED leds; // Not important, use defaults - Josiah
float sonar = 0.0;
// Now we have everything we need for a runswift request
ActionCommand::All request(head,body,leds,sonar);
// 3. Create runswift sensor object
SensorValues sensors;
// 3.1 Sense Joints
for (int ut_ind=0; ut_ind<NUM_JOINTS; ut_ind++){
if (ut_ind != RHipYawPitch){ // RS does not have this joint because RHYP is same as LHYP on Nao
int rs_ind = utJointToRSJoint[ut_ind];
sensors.joints.angles[rs_ind] = joints_->values_[ut_ind] * robot_joint_signs[ut_ind]; // changed from raw_joints - Josiah
sensors.joints.temperatures[rs_ind] = sensors_->joint_temperatures_[ut_ind];
}
}
// Detect if legs are getting hot
if(sensors_->joint_temperatures_[RKneePitch] > 100 || sensors_->joint_temperatures_[RHipPitch] > 100)
bodyModel.isRightLegHot = true;
if(sensors_->joint_temperatures_[LKneePitch] > 100 || sensors_->joint_temperatures_[LHipPitch] > 100)
bodyModel.isLeftLegHot = true;
// 3.2 Sensors
for (int ut_ind=0; ut_ind<bumperRR + 1; ut_ind++){
int rs_ind = utSensorToRSSensor[ut_ind];
sensors.sensors[rs_ind] = sensors_->values_[ut_ind];
}
// 4. If robot is remaining still, calbrate x and y gyroscopes
// 4.1 Calibrate gyroX:
double cur_gyroX = sensors.sensors[RSSensors::InertialSensor_GyrX];
double delta_gyroX = abs(cur_gyroX - last_gyroX);
// maintain moving averages for gyro and delta_gyro
avg_gyroX = avg_gyroX * (1.0 - window_size) + cur_gyroX * window_size;
avg_delta_gyroX = avg_delta_gyroX * (1.0- window_size) + delta_gyroX * window_size;
if (avg_delta_gyroX < delta_threshold) {
// robot remains still, do calibration
offsetX = avg_gyroX;
// reset avg_delta so it does not keep recalibrating
avg_delta_gyroX = reset;
calX_count += 1;
if (calX_count == 1) {
cout << "(First calibration, may not be accurate) A GyroX calibration was triggered, offsetX set to: " << offsetX << endl;
}
else {
cout << "A GyroX calibration was triggered, offsetX set to: " << offsetX << endl;
}
last_gyroX_time = frame_info_-> seconds_since_start;
}
else {
if (DEBUG_OUTPUT) cout << "avg_delta_gyroX is: " << avg_delta_gyroX << " GyroX not stable, no calibration" << endl;
}
last_gyroX = cur_gyroX;
// 4.2 Calibrate gyroY:
double cur_gyroY = sensors.sensors[RSSensors::InertialSensor_GyrY];
double delta_gyroY = abs(cur_gyroY - last_gyroY);
// maintain moving averages for gyro and delta_gyro
avg_gyroY = avg_gyroY * (1.0 - window_size) + cur_gyroY * window_size;
avg_delta_gyroY = avg_delta_gyroY * (1.0- window_size) + delta_gyroY * window_size;
if (avg_delta_gyroY < delta_threshold) {
// robot remains still, do calibration
offsetY = avg_gyroY;
// reset avg_delta so it does not keep recalibrating
avg_delta_gyroY = reset;
calY_count += 1;
if (calY_count == 1) {
cout << "(First calibration, may not be accurate) A GyroY calibration was triggered, offsetY set to: " << offsetY << endl;
}
else {
cout << "A GyroY calibration was triggered, offsetY set to: " << offsetY << endl;
}
last_gyroY_time = frame_info_-> seconds_since_start;
}
else {
if (DEBUG_OUTPUT) cout << "avg_delta_gyroY is: " << avg_delta_gyroY << " GyroY not stable, no calibration" << endl;
}
last_gyroY = cur_gyroY;
// After each gyro has completed two cycles it is calibrated.
if (calX_count >= 2)
bodyModel.isGyroXCalibrated = true;
if (calY_count >= 2)
bodyModel.isGyroYCalibrated = true;
if ((calX_count >= 2 && calY_count >= 2) || (getSystemTime() - calibration_write_time < 600))
{
odometry_->walkDisabled = false;
calX_count = 2;
calY_count = 2;
if (last_gyroY_time > last_calibration_write + 30 && last_gyroX_time > last_calibration_write + 30)
{
calibration_write_time = getSystemTime();
GyroConfig config;
config.offsetX = offsetX;
config.offsetY = offsetY;
config.calibration_write_time = calibration_write_time;
config.saveToFile("gyro_calibration.txt");
last_calibration_write = frame_info_->seconds_since_start;
}
} else if ((calX_count < 2 || calY_count < 2)) {
odometry_->walkDisabled = true;
}
// Apply offset and convert from rad/sec to rad /frame
sensors.sensors[RSSensors::InertialSensor_GyrX] = (sensors.sensors[RSSensors::InertialSensor_GyrX] - offsetX) * 0.01;
sensors.sensors[RSSensors::InertialSensor_GyrY] = (sensors.sensors[RSSensors::InertialSensor_GyrY] - offsetY) * 0.01;
// 5. Prepare BodyModel to pass to makeJoints
// 5.1 Get Kinematics ready for bodyModel. We should figure out what parameter values should be.
kinematics.setSensorValues(sensors);
// TODO apply camera calibrations to kinematics. For now we use default of 0
// kinematics.parameters.cameraPitchTop=2.4;
// kinematics.parameters.cameraYawTop=sensors_->values_[HeadYaw];
// kinematics.parameters.cameraRollTop=0;
// kinematics.parameters.cameraYawBottom=sensors_->values_[HeadPitch];
// kinematics.parameters.cameraPitchBottom=sensors_->values_[HeadYaw];
// kinematics.parameters.cameraRollBottom=0;
// kinematics.parameters.bodyPitch=3.3;//sensors_->values_[angleY];
kinematics.updateDHChain();
// Resetting generators and moving to intermediate stance
if (standing){
clipper->reset();
if(DEBUG_OUTPUT) cout << "Resetting clipper" << endl;
request.body = ActionCommand::Body::INITIAL;
odo.clear();
}
// 5.2 update bodyModel
bodyModel.kinematics = &kinematics;
bodyModel.update(&odo, sensors);
// 6. Get ball position relative to robot
// This is for lining up to the ball
WorldObject* ball = &(world_objects_->objects_[WO_BALL]); //ball->loc.x (after localization)
double ballX = ball->relPos.x;
double ballY = ball->relPos.y;
if(DEBUG_OUTPUT) cout << "BallX: "<<ballX<<" BallY: "<<ballY<<endl;
if (request.body.actionType == ActionCommand::Body::WALK && odometry_->walkDisabled){
static int delay_ct = 0;
if (delay_ct % 100 == 0)//prev_command != walk_request_->motion_)
speech_->say("Not Calibrated");
delay_ct ++;
if(DEBUG_OUTPUT) cout << "Not calibrated" << endl;
if (odometry_->standing)
request.body = ActionCommand::Body::STAND;
else
request.body = ActionCommand::Body::NONE;
}
if(DEBUG_OUTPUT) printf("motion request: %s, prev: %s, body request: %i\n", WalkRequestBlock::getName(walk_request_->motion_), WalkRequestBlock::getName(prev_command), request.body);
prev_command = walk_request_->motion_;
// Call the clipped generator which calls the distributed generator to produce walks, stands, etc.
JointValues joints = clipper->makeJoints(&request, &odo, sensors, bodyModel, ballX, ballY);
// Printing out current_generator for debugging
/* ActionCommand::Body::ActionType action_type = ((DistributedGenerator*)(clipper->generator))->current_generator;
switch(action_type){
case ActionCommand::Body::ActionType::STAND_STRAIGHT:
cout << "STAND_STRAIGHT"<<endl; break;
case ActionCommand::Body::ActionType::STAND:
cout << "STAND"<<endl; break;
case ActionCommand::Body::ActionType::WALK:
cout << "WALK"<<endl; break;
case ActionCommand::Body::ActionType::INITIAL:
cout << "INITIAL" << endl; break;
case ActionCommand::Body::ActionType::NONE:
cout << "NULL GEN" <<endl; break;
default:
cout << "Another Generator: "<<action_type<<endl;
}
*/
// We aren't setting arms any more so this isn't important unless we go back to it
// Setting arms behind back makes us get caught less but then we can't counter balance rswalk
if ((frame_info_->seconds_since_start - last_walk_or_stand_) > 0.3) {
arm_state_ = -1;
}
// Update odometry
static double cum_f=0, cum_l=0, cum_t=0;
odometry_->displacement.translation.x = odo.forward;
odometry_->displacement.translation.y = odo.left;
odometry_->displacement.rotation = odo.turn;
//if (walk_request_->speed_.translation.x > 0.5)
//odometry_->displacement.rotation -= (walk_params_->bh_params_.rs_turn_angle_offset / 100.0);
odometry_->standing = clipper->isStanding();
Odometry delta = Odometry(odo - prev);
cum_f += delta.forward;
cum_l += delta.left;
cum_t += delta.turn;
// For debugging odometry
/* if (body.turn != 0){
cout << "Odometry Prev: " << prev.turn;
cout << " Odometry Delta: " << delta.turn;
cout << " Odometry: " << odo.turn;
cout << " Cumulative Odometry: " << cum_t << endl;
}
*/
// Update walk_info_
//walk_info_->walk_is_active_ = !(clipper->isStanding());
walk_info_->instability_ = avg_gyroX;
if (avg_gyroX < -2 or avg_gyroX > 2) {
walk_info_->instable_ = true;
} else {
walk_info_->instable_ = false;
}
// Update
wasKicking = kick_request_->kick_running_;
if (request.body.actionType == ActionCommand::Body::ActionType::NONE){
return;
}
// For setting arms
last_walk_or_stand_ = frame_info_->seconds_since_start;
// Convert RS joints to UT joints and write to commands memory block
for (int ut_ind = BODY_JOINT_OFFSET; ut_ind < NUM_JOINTS; ut_ind++) {
if (ut_ind != RHipYawPitch){ // RS does not have this joint - Josiah
int rs_ind = utJointToRSJoint[ut_ind];
commands_->angles_[ut_ind] = robot_joint_signs[ut_ind] * joints.angles[rs_ind];
commands_->stiffness_[ut_ind] = joints.stiffnesses[rs_ind];
}
}
// Ruohan: setting head stiffness was commented out in bhuman. Should ask Jake about this
commands_->stiffness_[HeadPitch] = 1.0;
commands_->stiffness_[HeadYaw] = 1.0;
commands_->stiffness_[RHipYawPitch] = commands_->stiffness_[LHipYawPitch]; // RHYP will be overwritten with LHYP anyway but for completeness
commands_->angles_[RHipYawPitch] = commands_->angles_[LHipYawPitch];
// Walk keeps falling backwards when knees over 100. Leaning forward helps some
if (request.body.actionType == ActionCommand::Body::STAND && (bodyModel.isLeftLegHot || bodyModel.isRightLegHot)){
commands_->angles_[RHipPitch] -= DEG_T_RAD * 2;
commands_->angles_[LHipPitch] -= DEG_T_RAD * 2;
}
// if the robot has walked, listen to arm command from WalkGenerator, else do stiffness
if (request.body.actionType != ActionCommand::Body::WALK) {
for (int ind = ARM_JOINT_FIRST; ind <= ARM_JOINT_LAST; ind ++)
commands_->stiffness_[ind] = 1.0; //0.75;
}
selectivelySendStiffness(); // Only send stiffness if at least one joint has changed stiffness values by at least 0.01
// setArms(commands_->angles_,0.01); // Arms getting stuck on robot front with rswalk. Needs tuning
commands_->send_body_angles_ = true; // So commands will execute
commands_->send_stiffness_ = true;
if (request.body.actionType == ActionCommand::Body::STAND){
commands_->body_angle_time_ = 10; // Increase these if standing?
commands_->stiffness_time_ = 10;
} else {
commands_->body_angle_time_ = 10;
commands_->stiffness_time_ = 10;
}
// Not doing anything with these now. Could be used in future for smoothing
prevForward = body.forward;
prevLeft = body.left;
prevTurn = body.turn;
// Agent Effector code?
// This is in rswalk agent effecter. Not sure what standing is but it works now.
static bool kill_standing = false;
standing = kill_standing;
if (kill_standing) {
kill_standing = false;
standing = false;
}
else{
kill_standing = true;
}
}
TEST(JanethOdometryCalibrationTestSuite, testOdometry) {
Odometry odometry;
ASSERT_EQ(odometry.getPose(), Eigen::Vector3d::Zero());
ASSERT_EQ(odometry.getTimestamp(), 0);
ASSERT_EQ(odometry.getPoses(), std::vector<Eigen::Vector3d>(
{Eigen::Vector3d::Zero()}));
ASSERT_EQ(odometry.getTimestamps(), std::vector<double>({0}));
odometry.updateCOGDisplacement(0.5, 0, 5);
ASSERT_EQ(odometry.getPose(), Eigen::Vector3d(0.5, 0, 0));
ASSERT_EQ(odometry.getTimestamp(), 5);
ASSERT_THROW(odometry.updateCOGDisplacement(0, M_PI / 4, 0),
std::runtime_error);
odometry.updateCOGDisplacement(0, M_PI / 4, 6);
ASSERT_EQ(odometry.getPose(), Eigen::Vector3d(0.5, 0, M_PI / 4));
ASSERT_EQ(odometry.getTimestamp(), 6);
odometry.reset();
ASSERT_EQ(odometry.getPose(), Eigen::Vector3d::Zero());
ASSERT_EQ(odometry.getTimestamp(), 0);
odometry.updateCOGVelocity(0.0, 0.0, 0.0);
odometry.updateCOGVelocity(0.1, 0, 10);
ASSERT_EQ(odometry.getPose(), Eigen::Vector3d(1, 0, 0));
ASSERT_EQ(odometry.getTimestamp(), 10);
odometry.updateCOGVelocity(0.0, M_PI / 16, 18);
ASSERT_EQ(odometry.getPose(), Eigen::Vector3d(1, 0, M_PI / 2));
ASSERT_EQ(odometry.getTimestamp(), 18);
odometry.insertPose(Eigen::Vector3d(1, 1, M_PI / 2), 19);
ASSERT_EQ(odometry.getPose(), Eigen::Vector3d(1, 1, M_PI / 2));
ASSERT_EQ(odometry.getTimestamp(), 19);
ASSERT_THROW(odometry.insertPose(Eigen::Vector3d(1, 1, M_PI / 2), 10),
std::runtime_error);
odometry.reset(Eigen::Vector3d(1, 2, M_PI / 6), 10);
ASSERT_EQ(odometry.getPose(), Eigen::Vector3d(1, 2, M_PI / 6));
ASSERT_EQ(odometry.getTimestamp(), 10);
}
const Odometry& OmniDrivePositionController::computeNewOdometry(const Odometry& actualOdometry, double elapsedTimeInSec) {
KDL::Trajectory& trajectoryComposite = targetTrajectory.getTrajectory();
this->actualOdometry = actualOdometry;
computedOdometry = Odometry();
if (trajectoryComposite.Duration() > 0 && !isTargetReached()) {
actualTime = elapsedTimeInSec;
KDL::Frame desiredPose = trajectoryComposite.Pos(actualTime);
KDL::Twist desiredTwist = trajectoryComposite.Vel(actualTime);
KDL::Frame initPose = trajectoryComposite.Pos(0);
double r, p, y;
desiredPose.M.GetRPY(r, p, y);
Odometry desiredOdometryGlobal(Pose2D(desiredPose.p.x(),
desiredPose.p.y(),
actualOdometry.getPose2D().getTheta()),
Twist2D(desiredTwist.vel.x(),
desiredTwist.vel.y(),
desiredTwist.rot.z()));
Odometry desiredOdometryLocal;
Odometry actualOdometryGlobal(actualOdometry);
Odometry actualOdometryLocal;
OmniDriveKinematicsModel omnidrive;
omnidrive.convertToLocalReferenceFrame(desiredOdometryGlobal, desiredOdometryLocal);
omnidrive.convertToLocalReferenceFrame(actualOdometryGlobal, actualOdometryLocal);
double dPosX = desiredOdometryLocal.getPose2D().getX();
double dPosY = desiredOdometryLocal.getPose2D().getY();
double dPosTheta = y; //desiredOdometryLocal.getPose2D().getTheta();
double dVelX = desiredOdometryLocal.getTwist2D().getX();
double dVelY = desiredOdometryLocal.getTwist2D().getY();
double dVelTheta = desiredOdometryLocal.getTwist2D().getTheta();
double aPosX = actualOdometryLocal.getPose2D().getX();
double aPosY = actualOdometryLocal.getPose2D().getY();
double aPosTheta = actualOdometryLocal.getPose2D().getTheta();
double positionXError = dPosX - aPosX;
double positionYError = dPosY - aPosY;
double positionThetaError = getShortestAngle(dPosTheta, aPosTheta);
double gainPosX = gains.getPose2D().getX();
double gainPosY = gains.getPose2D().getY();
double gainPosTheta = gains.getPose2D().getTheta();
double gainVelX = gains.getTwist2D().getX();
double gainVelY = gains.getTwist2D().getY();
double gainVelTheta = gains.getTwist2D().getTheta();
double errorTheta = gainVelTheta * dVelTheta + gainPosTheta * positionThetaError;
double errorX = gainVelX * dVelX + gainPosX * positionXError;
double errorY = gainVelY * dVelY + gainPosY * positionYError;
computedOdometry.setTwist2D(Twist2D(errorX, errorY, errorTheta));
}
return computedOdometry;
}
