bool wex::stc_data::inject() const { if (m_STC == nullptr) return false; bool injected = m_Data.inject( [&]() { if (m_Data.line() > 0) { m_STC->GotoLine(m_Data.line() -1); m_STC->EnsureVisible(m_Data.line() -1); m_STC->EnsureCaretVisible(); m_STC->IndicatorClearRange(0, m_STC->GetTextLength() - 1); m_STC->set_indicator(indicator(0), m_STC->PositionFromLine(m_Data.line() -1), m_Data.col() > 0 ? m_STC->PositionFromLine(m_Data.line() -1) + m_Data.col() - 1: m_STC->GetLineEndPosition(m_Data.line() -1)); } else if (m_Data.line() == DATA_NUMBER_NOT_SET) { return false; } else { m_STC->DocumentEnd(); } return true;}, [&]() { const int max = (m_Data.line() > 0) ? m_STC->GetLineEndPosition(m_Data.line() - 1): 0; const int asked = m_STC->GetCurrentPos() + m_Data.col() - 1; m_STC->SetCurrentPos(asked < max ? asked: max); // Reset selection, seems necessary. m_STC->SelectNone(); return true;}, [&]() { if (m_Data.line() > 0) { const int start_pos = m_STC->PositionFromLine(m_Data.line() -1); const int end_pos = m_STC->GetLineEndPosition(m_Data.line() -1); m_STC->set_search_flags(-1); m_STC->SetTargetStart(start_pos); m_STC->SetTargetEnd(end_pos); if (m_STC->SearchInTarget(m_Data.find()) != -1) { m_STC->SetSelection(m_STC->GetTargetStart(), m_STC->GetTargetEnd()); } } else if (m_Data.line() == DATA_NUMBER_NOT_SET) { m_STC->find_next(m_Data.find(), m_Data.find_flags()); } else { m_STC->find_next(m_Data.find(), m_Data.find_flags(), false); } return true;}, [&]() { return m_STC->get_vi().command(m_Data.command().command());}); if (!m_Data.window().name().empty()) { m_STC->SetName(m_Data.window().name()); } if ( m_WinFlags[WIN_READ_ONLY] || (m_STC->get_filename().file_exists() && m_STC->get_filename().is_readonly())) { m_STC->SetReadOnly(true); injected = true; } if (m_STC->get_hexmode().set(m_WinFlags[WIN_HEX])) { injected = true; } if (injected) { m_STC->properties_message(); } return injected; }
bool Sisim::initialize( const Parameters_handler* parameters, Error_messages_handler* errors ) { // Extract the parameters input by the user from the parameter handler //------------- // The "simulation" grid parameters std::string simul_grid_name = parameters->value( "Grid_Name.value" ); errors->report( simul_grid_name.empty(), "Grid_Name", "No grid selected" ); std::string property_name = parameters->value( "Property_Name.value" ); errors->report( property_name.empty(), "Property_Name", "No property name specified" ); // Get the simulation grid from the grid manager bool ok = geostat_utils::create( simul_grid_, simul_grid_name, "Grid_Name", errors ); if( !ok ) return false; // create a multi-realization property multireal_property_ = simul_grid_->add_multi_realization_property( property_name ); appli_assert( multireal_property_ ); //------------- // Miscellaneous simulation parameters nb_of_realizations_ = String_Op::to_number<int>( parameters->value( "Nb_Realizations.value" ) ); seed_ = String_Op::to_number<int>( parameters->value( "Seed.value" ) ); //------------- // The hard data parameters std::string harddata_grid_name = parameters->value( "Hard_Data_Grid.value" ); // Get the harddata grid from the grid manager and select the // hard data property if( !harddata_grid_name.empty() ) { geostat_utils::create( harddata_grid_, harddata_grid_name, "Hard_Data_Grid", errors ); std::string hdata_property_name = parameters->value( "Hard_Data_Property.value" ); errors->report( hdata_property_name.empty(), "Hard_Data_Property", "No properties specified" ); // Get the harddata property from the grid harddata_property_ = harddata_grid_->property( hdata_property_name ); } bool assign_harddata = String_Op::to_number<bool>( parameters->value( "Assign_Hard_Data.value" ) ); if( harddata_grid_ == NULL ) assign_harddata=false; else if( harddata_grid_ == simul_grid_ ) assign_harddata=true; if( assign_harddata ) { property_copier_ = Property_copier_factory::get_copier( harddata_grid_, simul_grid_ ); if( !property_copier_ ) { std::ostringstream message; message << "It is currently not possible to copy a property from a " << harddata_grid_->classname() << " to a " << simul_grid_->classname() ; errors->report( !property_copier_, "Assign_Hard_Data", message.str() ); return false; } // initializer_ = new Grid_initializer( simul_grid_, false ); } is_data_coded_ = false; // Geostat_grid* coded_grid; std::string coded_grid_name = parameters->value( "coded_grid.value" ); if( !coded_grid_name.empty() ) { geostat_utils::create( coded_grid_, coded_grid_name, "coded_grid", errors ); } std::vector<GsTLGridProperty*> coded_props; std::string coded_prop_names = parameters->value( "coded_props.value" ); if(!coded_prop_names.empty() && coded_grid_ ) { is_data_coded_ = true; std::vector<std::string> coded_names = String_Op::decompose_string(coded_prop_names,";"); for(int i=0; i<coded_names.size() ; ++i ) { coded_props.push_back( coded_grid_->property( coded_names[i] ) ); errors->report(coded_props[i]==NULL,"coded_props","A property does not exist"); } } if( !errors->empty() ) return false; //------------- // The cdf parameters (# of thresholds, marginal, ...) int nb_indicators = String_Op::to_number<int>( parameters->value( "Nb_Indicators.value" ) ); if (nb_indicators < 2) { errors->report( "Nb_Indicators", "Must have at least 2 categories" ); return false; } if(is_data_coded_ && coded_props.size() != nb_indicators ) { std::ostringstream message; message << "enter exactly " << nb_indicators << " of soft data"; errors->report( "coded_props", message.str() ); return false; } is_categorical_ = String_Op::to_number<bool>( parameters->value( "Categorical_Variable_Flag.value" ) ); std::string marginal_probabilities_string = parameters->value( "Marginal_Probabilities.value" ); std::vector<double> marginal_probs = String_Op::to_numbers<double>( marginal_probabilities_string ); if( marginal_probs.size() != nb_indicators ) { std::ostringstream message; message << "enter exactly " << nb_indicators << " probabilities"; errors->report( "Marginal_Probabilities", message.str() ); return false; } if( is_categorical_ ) { // we ignore the input threshold values. The categories are numbered // from 0 to k (if there are k+1 categories). ccdf_ = new Categ_non_param_cdf<float>( marginal_probs.size() ); marginal_ = new Categ_non_param_cdf<float>( marginal_probs.size(), marginal_probs.begin() ); errors->report( !is_valid_cdf( marginal_probs.begin(), marginal_probs.end(), GsTL::discrete_variable_tag() ), "Marginal_Probabilities", "Values must be between 0 and 1 and sum up to 1" ); } else { std::vector<float> thresholds = String_Op::to_numbers<float>( parameters->value( "Thresholds.value" ) ); if( thresholds.size() != nb_indicators ) { std::ostringstream message; message << "enter exactly " << nb_indicators << " thresholds"; errors->report( "Thresholds", message.str() ); return false; } errors->report( !GsTL::is_sorted( thresholds.begin(), thresholds.end() ), "Thresholds", "Threshold values must be sorted in ascending order" ); ccdf_ = new Non_param_cdf<>( thresholds.begin(), thresholds.end() ); marginal_ = new Non_param_cdf<>( thresholds.begin(), thresholds.end(), marginal_probs.begin() ); errors->report( !is_valid_cdf( marginal_probs.begin(), marginal_probs.end(), GsTL::continuous_variable_tag() ), "Marginal_Probabilities", "The values entered do not define a valid cdf" ); geostat_utils::set_cdf_extrapolation_tail(parameters,errors, *((Non_param_cdf<>*) ccdf_), "lowerTailCdf", "upperTailCdf"); geostat_utils::set_cdf_extrapolation_tail(parameters,errors, *((Non_param_cdf<>*) marginal_), "lowerTailCdf", "upperTailCdf"); } // report errors found so far if( !errors->empty() ) return false; //------------- // The Ik parameter (median or full) do_median_ik_ = String_Op::to_number<bool>( parameters->value( "Median_Ik_Flag.value" ) ); //------------- // The search neighborhood parameters int max_neigh = String_Op::to_number<int>( parameters->value( "Max_Conditioning_Data.value" ) ); GsTLTriplet ranges; GsTLTriplet angles; bool extract_ok = geostat_utils::extract_ellipsoid_definition( ranges, angles, "Search_Ellipsoid.value", parameters, errors ); if( !extract_ok ) return false; extract_ok = geostat_utils::is_valid_range_triplet( ranges ); errors->report( !extract_ok, "Search_Ellipsoid", "Ranges must verify: major range >= " "medium range >= minor range >= 0" ); if( !extract_ok ) return false; //------------- // Variogram (covariance) and search ellipsoid initialization if( do_median_ik_ ) { // We're doing median ik: we only need one covariance and one // search neighborhood covar_vector_.resize( 1 ); bool init_cov_ok = geostat_utils::initialize_covariance( &covar_vector_[0], "Variogram_Median_Ik", parameters, errors ); if( !init_cov_ok ) return false; if( !harddata_grid_ || assign_harddata ) { neighborhood_ = SmartPtr<Neighborhood>( simul_grid_->neighborhood( ranges, angles, &covar_vector_[0] ) ); } else { Neighborhood* simul_neigh = simul_grid_->neighborhood( ranges, angles, &covar_vector_[0] ); Neighborhood* harddata_neigh; if( dynamic_cast<Point_set*>(harddata_grid_) ) { harddata_neigh = harddata_grid_->neighborhood( ranges, angles, &covar_vector_[0],true ); } else { harddata_neigh = harddata_grid_->neighborhood( ranges, angles, &covar_vector_[0] ); } harddata_neigh->max_size( max_neigh ); harddata_neigh->select_property( harddata_property_->name() ); neighborhood_ = SmartPtr<Neighborhood>( new Combined_neighborhood( harddata_neigh, simul_neigh, &covar_vector_[0]) ); } neighborhood_->max_size( max_neigh ); geostat_utils::set_advanced_search(neighborhood_, "AdvancedSearch", parameters, errors); } else { // we're doing full ik: we need as many covariances and neighborhoods as // indicators covar_vector_.resize( ccdf_->size() ); neighborhoods_vector_.resize( ccdf_->size() ); // initialize all the covariances bool init_cov_ok = geostat_utils::initialize_covariance( &covar_vector_[0], "Variogram_Full_Ik", parameters, errors ); if( !init_cov_ok ) return false; for( int i=1; i < ccdf_->size(); i++ ) { std::string tagname = "Variogram_Full_Ik_" + String_Op::to_string( i+1 ); init_cov_ok = geostat_utils::initialize_covariance( &covar_vector_[i], tagname, parameters, errors ); if( !init_cov_ok ) return false; } // initialize all the neighborhoods. for( int j=0; j < ccdf_->size(); j++ ) { SmartPtr<Neighborhood> coded_neigh; if( is_data_coded_ ) { if( dynamic_cast<Point_set*>(coded_grid_) ) { coded_neigh = SmartPtr<Neighborhood>( coded_grid_->neighborhood(ranges, angles, &covar_vector_[j],true )); } else { coded_neigh = SmartPtr<Neighborhood>( coded_grid_->neighborhood(ranges, angles, &covar_vector_[j] )); } coded_neigh->select_property( coded_props[j]->name() ); coded_neigh->max_size( max_neigh ); } else { coded_neigh = SmartPtr<Neighborhood>( new DummyNeighborhood() ); } if( !harddata_grid_ || assign_harddata ) { SmartPtr<Neighborhood> simul_neigh = SmartPtr<Neighborhood>( simul_grid_->neighborhood( ranges, angles, &covar_vector_[j] )); SmartPtr<Neighborhood> neighborhood = SmartPtr<Neighborhood>( new Combined_neighborhood_dedup( coded_neigh, simul_neigh, &covar_vector_[j],true) ); neighborhood->max_size( max_neigh ); neighborhoods_vector_[j] = NeighborhoodHandle( neighborhood ); } else { Neighborhood* simul_neigh = simul_grid_->neighborhood( ranges, angles, &covar_vector_[j] ); simul_neigh->max_size( max_neigh ); Neighborhood* harddata_neigh; if( dynamic_cast<Point_set*>(harddata_grid_) ) { harddata_neigh = harddata_grid_->neighborhood( ranges, angles, &covar_vector_[j],true ); } else { harddata_neigh = harddata_grid_->neighborhood( ranges, angles, &covar_vector_[j] ); } harddata_neigh->max_size( max_neigh ); harddata_neigh->select_property( harddata_property_->name() ); SmartPtr<Neighborhood> hard_soft_data_neigh = SmartPtr<Neighborhood>( new Combined_neighborhood( harddata_neigh, coded_neigh, &covar_vector_[j] ) ); hard_soft_data_neigh->max_size( max_neigh ); SmartPtr<Neighborhood> neighborhood = SmartPtr<Neighborhood>( new Combined_neighborhood( hard_soft_data_neigh, simul_neigh, &covar_vector_[j] ) ); neighborhood->max_size( max_neigh ); neighborhoods_vector_[j] = NeighborhoodHandle( neighborhood ); } geostat_utils::set_advanced_search(neighborhoods_vector_[j], "AdvancedSearch", parameters, errors); if( !errors->empty() ) return false; } } //------------- // Set-up the cdf estimator if( is_categorical_ ) { Indicator<double> indicator( new Class_indicator_function<double> ); /* cdf_estimator_ = new CdfEstimator( covar_vector_.begin(), covar_vector_.end(), marginal_->p_begin(), marginal_->p_end(), indicator );*/ cdf_estimator_ = new CdfSoftEstimator( covar_vector_.begin(), covar_vector_.end(), marginal_->p_begin(), marginal_->p_end(), indicator,harddata_property_ ); } else { // The user has no pre-coded data; /* if( !is_data_coded_ ) { Indicator<double> indicator( new Indicator_function<double> ); cdf_estimator_ = new CdfEstimator( covar_vector_.begin(), covar_vector_.end(), marginal_->p_begin(), marginal_->p_end(), indicator ); } // There is some pre-coded data else {*/ Indicator<double> indicator( new Indicator_function<double> ); //cdf_soft_estimator_ = cdf_estimator_ = new CdfSoftEstimator( covar_vector_.begin(), covar_vector_.end(), marginal_->p_begin(), marginal_->p_end(), indicator,harddata_property_ ); // } } // Set up the regions std::string region_name = parameters->value( "Grid_Name.region" ); if (!region_name.empty() && simul_grid_->region( region_name ) == NULL ) { errors->report("Grid_Name","Region "+region_name+" does not exist"); } else grid_region_.set_temporary_region( region_name, simul_grid_); if(harddata_grid_ && !assign_harddata && harddata_grid_ != simul_grid_) { region_name = parameters->value( "Hard_Data_Grid.region" ); if (!region_name.empty() && harddata_grid_->region( region_name ) == NULL ) { errors->report("Hard_Data_Grid","Region "+region_name+" does not exist"); } else hd_grid_region_.set_temporary_region( region_name,harddata_grid_ ); } if(coded_grid_ && coded_grid_ != simul_grid_ && coded_grid_ != harddata_grid_) { region_name = parameters->value( "coded_grid.region" ); if (!region_name.empty() && coded_grid_->region( region_name ) == NULL ) { errors->report("coded_grid","Region "+region_name+" does not exist"); } else coded_grid_region_.set_temporary_region( region_name,harddata_grid_ ); } //------------------------------- // Done! if( !errors->empty() ) return false; return true; }
static double b_0(double t) { return -bpremium * indicator(t >= 0) * indicator(t < n); }
void motion_controller::running(void) { cycle_start = ros::Time::now(); // receive path and when received block path receiver until mission completed if (m_path && !block_path_receiver) { nav_msgs::Path temp_path = *m_path; std::vector<geometry_msgs::PoseStamped> temp_path_vector; extracted_path.clear(); for( std::vector<geometry_msgs::PoseStamped>::iterator itr = temp_path.poses.begin() ; itr != temp_path.poses.end(); ++itr) { temp_path_vector.push_back(*itr); } if(!temp_path_vector.empty()) { ROS_INFO("Path Received!"); } p_count++; while(!temp_path_vector.empty()) { extracted_path.push_back(temp_path_vector.back()); temp_path_vector.pop_back(); } block_path_receiver = true; } // switch status according to the command switch (m_move) { // move forward case FORWARD: { // v is linear speed and gain is angular velocity v = sqrt(dist)*v_max/(1 + gamma*k*k)*10000/PI; gain = 0.3265*v*k; if (fabs(gain) > 600) { gain = boost::math::sign(gain) * 600; } if(fabs(v)>2000) { v = boost::math::sign(v)*2000; } // set motor rotation velocity locomotor->set_vel(floor(v)+floor(gain),-floor(v)+floor(gain)); // when the euler distance is less than 100mm, achieved waypoint if (distance2d(m_state, m_cmd) < 0.09 && indicator(m_state[2],m_cmd[2],0.5)) { m_move = IDLE; } break; } case BACKWARD: { v = sqrt(dist) * 0.75/(1+0.2*k_back*k_back) *10000/3.1415926; gain = 0.3265*v*k_back; if (fabs(gain) > 600) { gain = boost::math::sign(gain) * 600; } if(fabs(v)>2000) { v = boost::math::sign(v)*2000; } locomotor->set_vel(-floor(v)+floor(gain),floor(v)+floor(gain)); if (distance2d(m_state, m_cmd) < 0.09 && indicator(m_state[2],m_cmd[2],0.5)) { m_move = IDLE; } break; } case LIFTFORK: { // TODO: Lift height should be determined, fork stroke should be determined printf("Lift and fork!\n"); locomotor->set_vel(0, 0); sleep(3); // locomotor->shut_down(); lift_motor->power_on(); if ((fork_count%2) == 1) // change -243720 to be -223720, wants to liftfork a little bit lower lift_motor->set_pos(-223720); else lift_motor->set_pos(-293720); sleep(10); printf("Start Test\n"); // pose correction code inserted here first make sure tag is attached vertically, second camera has no pitch angle relative to the vehicle if ((fork_count%2) == 1) { printf("Start Correction!\n"); int count_detect = 0; while(ros::ok()) { if(abs_pose1) { Eigen::Quaternion<float> quat; quat.w() = abs_pose1->pose.pose.orientation.w; quat.x() = abs_pose1->pose.pose.orientation.x; quat.y() = abs_pose1->pose.pose.orientation.y; quat.z() = abs_pose1->pose.pose.orientation.z; Eigen::Matrix3f Rotation; Rotation = qToRotation(quat); Eigen::Matrix3f FixTF; FixTF << 1, 0, 0, 0, 0, -1, 0, 1, 0; Rotation = FixTF * Rotation.inverse(); float yaw_error; yaw_error = getYawFromMat(Rotation); gain = -3265*(k_angle*yaw_error)/3.1415926; if(fabs(gain)>150) { gain = boost::math::sign(gain) * 150; } locomotor->set_vel(floor(gain), floor(gain)); if (fabs(yaw_error*180/3.1415926) < 0.5) { locomotor->set_vel(0,0); printf("Yaw %f corrected!\n", yaw_error*180/3.1415926); break; } } else { locomotor->set_vel(0, 0); usleep(10000); count_detect++; } ros::spinOnce(); if (count_detect>1000) { ROS_WARN("No Tag detected when stoped"); ros::shutdown(); exit(1); } } count_detect = 0; while(ros::ok()) { if(abs_pose1) { Eigen::Quaternion<float> quat; quat.w() = abs_pose1->pose.pose.orientation.w; quat.x() = abs_pose1->pose.pose.orientation.x; quat.y() = abs_pose1->pose.pose.orientation.y; quat.z() = abs_pose1->pose.pose.orientation.z; Eigen::Matrix3f Rotation; Eigen::Vector3f translation; translation << abs_pose1->pose.pose.position.x, abs_pose1->pose.pose.position.y, abs_pose1->pose.pose.position.z; Rotation = qToRotation(quat); translation = translation; float x_error; x_error = translation[0]; v = 0.25*(x_error-0.003)*10000/3.1415926; // printf("x is %f\n", x_error); // printf("v is %f\n\n", floor(v)); if(fabs(v)>150) { v = boost::math::sign(v) * 150; } locomotor->set_vel(floor(v), -floor(v)); if (fabs(x_error-0.003) < 0.003) { locomotor->set_vel(0, 0); printf("x %f corrected!\n", (x_error-0.006)); break; } } else { locomotor->set_vel(0, 0); usleep(10000); count_detect++; } ros::spinOnce(); if (count_detect>1000) { ROS_WARN("No Tag detected when stoped"); ros::shutdown(); exit(1); } } } if ((fork_count%2) == 0) { int count_detect = 0; while(ros::ok()) { if (abs_pose) { Eigen::Quaternion<float> quat; quat.w() = abs_pose->pose.pose.orientation.w; quat.x() = abs_pose->pose.pose.orientation.x; quat.y() = abs_pose->pose.pose.orientation.y; quat.z() = abs_pose->pose.pose.orientation.z; Eigen::Matrix3f Rotation; Rotation = qToRotation(quat); Eigen::Matrix3f fixTF; fixTF << 1, 0, 0, 0, -1, 0, 0, 0, -1; Rotation = Rotation.inverse()*fixTF; m_state[2] = getYawFromMat(Rotation); gain = 3265*(k_angle*angle(m_cmd[2],m_state[2]))/PI; if (fabs(gain) > 100) { gain = boost::math::sign(gain) * 100; } if (fabs(gain) >100) { ros::shutdown(); exit(1); } locomotor->set_vel(floor(gain),floor(gain)); if (indicator(m_cmd[2], m_state[2], 0.008)) { locomotor->set_vel(0, 0); printf("Corrected!\n"); break; } } else { locomotor->set_vel(0, 0); usleep(10000); count_detect++; } ros::spinOnce(); if (count_detect>1000) { locomotor->set_vel(0, 0); ROS_WARN("No Tag detected when stoped"); //ros::shutdown(); //exit(1); } } } // if we carry out hand hold barcode test, after correction, stop // ros::shutdown(); // exit(1); locomotor->shut_down(); sleep(0.5); // comment following two lines can set make telefork not strech out telefork_motor->set_pos( boost::math::sign(lift_param)*(-386000) ); sleep(15); if ((fork_count%2) == 1) lift_motor->set_pos(-293720); else lift_motor->set_pos(-223720); sleep(3); telefork_motor->set_pos(0); sleep(15); // ros::shutdown(); // exit(1); lift_motor->shut_down(); locomotor->power_on(); sleep(0.5); m_move = IDLE; break; } case TURN: { v = cos(alpha) * dist* k_dist *10000/PI; if(fabs(v)>300) { v = boost::math::sign(v)*300; } gain = 3265*(k_angle*angle(m_cmd[2],m_state[2]))/PI; if (fabs(gain) > 400) { gain = boost::math::sign(gain) * 400; } locomotor->set_vel(floor(v)+floor(gain),-floor(v)+floor(gain)); if (indicator(m_state[2],m_cmd[2],0.01)) { m_move = IDLE; } break; } case IDLE: { locomotor->set_vel(0,0); if (extracted_path.size()!=0) { geometry_msgs::PoseStamped temp_pose = extracted_path.back(); float yaw_ = 2*atan2(temp_pose.pose.orientation.z,temp_pose.pose.orientation.w); m_cmd << temp_pose.pose.position.x * m_resolution, temp_pose.pose.position.y * m_resolution, angle(yaw_,0); printf("Next Commanded Pose is (%f, %f, %f)\n", m_cmd[0], m_cmd[1], m_cmd[2]); if ( (fabs(m_cmd[0] - m_state[0])>0.5) && (fabs(m_cmd[1] - m_state[1])>0.5) ) { printf("Invalid commanded position received!\n"); locomotor->shut_down(); ros::shutdown(); exit(1); } if ( (fabs(m_cmd[0] - m_state[0])>0.5) || (fabs(m_cmd[1] - m_state[1])>0.5) ) { if (fabs(angle(m_cmd[2],m_state[2]))>0.5) { printf("Invalid commanded pose orientation received!\n"); locomotor->shut_down(); ros::shutdown(); exit(1); } if (fabs(m_cmd[0] - m_state[0])>0.5) { if (cos(m_state[2]) * (m_cmd[0] - m_state[0]) > 0) m_move = FORWARD; else m_move = BACKWARD; } else { if (sin(m_state[2]) * (m_cmd[1] - m_state[1]) > 0) m_move = FORWARD; else m_move = BACKWARD; } if (m_move == FORWARD) printf("Move Forward!\n"); else printf("Move Backward!\n"); } else if (fabs(m_cmd[2] - m_state[2])>0.5) { m_move = TURN; printf("Turn Around!\n"); } else if (temp_pose.pose.position.z!=0) { m_move = LIFTFORK; fork_count++; lift_param = temp_pose.pose.position.z; } else m_move = IDLE; extracted_path.pop_back(); } else { if (p_count == 2) { lift_motor->power_on(); lift_motor->set_pos(0); sleep(12); lift_motor->shut_down(); sleep(0.5); } block_path_receiver = false; geometry_msgs::PoseStamped pose_msg; pose_msg.header.frame_id = "world"; pose_msg.header.stamp = ros::Time::now(); pose_msg.pose.position.x = m_state[0]; pose_msg.pose.position.y = m_state[1]; if (block_path_receiver) pose_msg.pose.position.z = 1; else pose_msg.pose.position.z = 0; pose_msg.pose.orientation.w = cos(m_state[2]/2); pose_msg.pose.orientation.x = 0; pose_msg.pose.orientation.y = 0; pose_msg.pose.orientation.z = sin(m_state[2]/2); m_posePub.publish(pose_msg); printf("IDLE!\n"); sleep(1); m_move = IDLE; } break; } } vel.first = (vel.first + locomotor->get_vel().first)/2; vel.second = (vel.second + locomotor->get_vel().second)/2; cycle_period = (ros::Time::now() - cycle_start).toSec(); m_state[0] = m_state[0] + (-vel.second+vel.first)/2 * cos(m_state[2]) * 0.018849555 * cycle_period/60; m_state[1] = m_state[1] + (-vel.second+vel.first)/2 * sin(m_state[2]) * 0.018849555 * cycle_period/60; m_state[2] = m_state[2] + (vel.first + vel.second)/0.653 * 0.018849555 * cycle_period/60; if (abs_pose) { int id; id = abs_pose->id - 3; Eigen::Quaternion<float> quat; quat.w() = abs_pose->pose.pose.orientation.w; quat.x() = abs_pose->pose.pose.orientation.x; quat.y() = abs_pose->pose.pose.orientation.y; quat.z() = abs_pose->pose.pose.orientation.z; Eigen::Matrix3f Rotation; Eigen::Vector3f translation; translation << abs_pose->pose.pose.position.x, abs_pose->pose.pose.position.y, abs_pose->pose.pose.position.z; Rotation = qToRotation(quat); translation = -Rotation.inverse()*translation; Eigen::Matrix3f fixTF; fixTF << 1, 0, 0, 0, -1, 0, 0, 0, -1; Rotation = Rotation.inverse()*fixTF; m_state[0] = translation[0] + m_resolution * (id%10); m_state[1] = translation[1] + m_resolution * floor(id/10.0); m_state[2] = getYawFromMat(Rotation) + 0.04; } geometry_msgs::PoseStamped pose_msg; pose_msg.header.frame_id = "world"; pose_msg.header.stamp = ros::Time::now(); pose_msg.pose.position.x = m_state[0]; pose_msg.pose.position.y = m_state[1]; if (block_path_receiver) pose_msg.pose.position.z = 1; else pose_msg.pose.position.z = 0; pose_msg.pose.orientation.w = cos(m_state[2]/2); pose_msg.pose.orientation.x = 0; pose_msg.pose.orientation.y = 0; pose_msg.pose.orientation.z = sin(m_state[2]/2); m_posePub.publish(pose_msg); delta_y = m_cmd[1]-m_state[1]; if(fabs(m_cmd[1]-m_state[1]) > 1.6) { delta_y = boost::math::sign(m_cmd[1]-m_state[1]) * 1.6; } delta_x = m_cmd[0]-m_state[0]; if(fabs(m_cmd[0]-m_state[0]) > 1.6) { delta_x = boost::math::sign(m_cmd[0]-m_state[0]) * 1.6; } beta = angle(m_cmd[2], atan2(delta_y, delta_x)); alpha = angle(m_state[2], atan2(delta_y, delta_x)); beta1 = angle(m_cmd[2]+PI, atan2(delta_y, delta_x)); alpha1 = angle(m_state[2]+3.1415926, atan2(delta_y, delta_x)); dist = sqrt(delta_x*delta_x + delta_y* delta_y); k = -1/dist * (k2*(alpha-atan(-k1*beta)) + sin(alpha)*(1 + k1/(1+(k1*beta) * (k1*beta)))); k_back = -1/dist * (k2*(alpha1-atan2(-k1*beta1,1)) + sin(alpha1)*(1 + k1/(1+(k1*beta1) * (k1*beta1)))); }
int AtIndicators::value( const QString& name ) const { return value( indicator( name ) ); }
void AtIndicators::setValue( const QString& name, int value ) { setValue( indicator( name ), value ); }