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 );
}
