void Wave::createAnimation()
{
float /*origin_x, origin_y,*/ distance, height, ripple_interval = 1.3;
fillCubeArray(0x00);
// QVector<QVector<QVector<quint8> > > tripleVector(iterations(),QVector<QVector<quint8> >(8, QVector<quint8>(8)));
for (quint16 i=0; i<iterations(); i++)
{
if(m_abort)
return;
for (quint8 x=0; x<8; x++)
{
for (quint8 y=0; y<8; y++)
{
distance = distance2d(3.5,3.5,x,y)/9.899495*8;
height = 4+sin(distance/ripple_interval+static_cast<float>(i)/50)*4;
setBixel(x,y,static_cast<quint8>(height));
}
}
waitMs(speed());
// tripleVector[i] = cubeFrame;
fillCubeArray(0x00);
}
// for (int i = 0; i < iterations(); i++) {
// sendData(tripleVector[i]);
//// if(i/10 == 0)
//// waitMs(speed());
// }
Q_EMIT done();
}
void sidewaves (int iterations, int delay)
{
float origin_x, origin_y, distance, height, ripple_interval;
int x,y,i;
fill(0x00);
for (i=0;i<iterations;i++)
{
origin_x = 3.5+sin((float)i/500)*4;
origin_y = 3.5+cos((float)i/500)*4;
for (x=0;x<8;x++)
{
for (y=0;y<8;y++)
{
distance = distance2d(origin_x,origin_y,x,y)/9.899495*8;
ripple_interval =2;
height = 4+sin(distance/ripple_interval+(float) i/50)*3.6;
setvoxel(x,y,(int) height);
setvoxel(x,y,(int) height);
}
}
delay_ms(delay);
fill(0x00);
}
}
// Display a sine wave running out from the center of the cube.
void ripples (int iterations, int delay)
{
float origin_x, origin_y, distance, height, ripple_interval;
int x,y,i;
fill(0x00);
for (i=0;i<iterations;i++)
{
for (x=0;x<8;x++)
{
for (y=0;y<8;y++)
{
distance = distance2d(3.5,3.5,x,y)/9.899495*8;
//distance = distance2d(3.5,3.5,x,y);
ripple_interval =1.3;
height = 4+sin(distance/ripple_interval+(float) i/50)*4;
setvoxel(x,y,(int) height);
}
}
delay_ms(delay);
fill(0x00);
}
}
void LocalPlanner::createPolarHistogram()
{
float bbx_rad = (max.x-min.x)*sqrt(2)/2;
float dist;
polar_histogram.setZero();
pcl::PointCloud<pcl::PointXYZ>::const_iterator it;
for( it = final_cloud.begin(); it != final_cloud.end(); ++it)
{
geometry_msgs::Point temp;
temp.x= it->x;
temp.y = it->y;
temp.z = it->z;
dist = distance2d(pose,temp);
if(dist < bbx_rad)
{
int beta_z = floor((atan2(temp.x-pose.x,temp.y-pose.y)*180.0/PI)); //azimuthal angle
int beta_e = floor((atan2(temp.z-pose.z,sqrt((temp.x-pose.x)*(temp.x-pose.x)+(temp.y-pose.y)*(temp.y-pose.y)))*180.0/PI));//elevation angle
beta_z = beta_z + (alpha_res - beta_z%alpha_res);
beta_e = beta_e + (alpha_res - beta_e%alpha_res);
int e = (180+beta_e)/alpha_res - 1 ;
int z = (180+beta_z)/alpha_res - 1 ;
polar_histogram.set(e,z,polar_histogram.get(e,z)+1);
}
}
}
void build_buildings(region_t ®ion, rltk::random_number_generator &rng, const int n_buildings, const bool active,
std::vector<std::tuple<int,int,int>> &spawn_points, const std::size_t &civ_id, planet_t &planet)
{
std::cout << "Civ ID#: " << civ_id << "\n";
int i=0;
while (i<n_buildings) {
auto hut = get_building_template(civ_id, planet, rng);
xp::rex_sprite * building = &hut;
bool ok = false;
int tries = 0;
int x,y,z;
while (!ok) {
ok = true;
x = rng.roll_dice(1, REGION_WIDTH - 20) + 10;
y = rng.roll_dice(1, REGION_HEIGHT - 20) + 10;
z = get_ground_z(region, x, y);
// Find lowest point
for (int Y=0; Y < building->get_height(); ++Y) {
for (int X = 0; X < building->get_width(); ++X) {
const int tmp_z = get_ground_z(region, x+X, y+Y);
if (z > tmp_z) z = tmp_z;
}
}
// Check for solid or underwater
for (int Y=0; Y < building->get_height(); ++Y) {
for (int X = 0; X < building->get_width(); ++X) {
for (int Z = 0; Z < building->get_num_layers(); ++Z) {
const auto idx = mapidx(X+x, Y+y, Z+z);
if (region.solid[idx]) ok = false;
if (region.water_level[idx] > 0) ok = false;
if (region.tile_type[idx] == tile_type::SOLID) ok = false;
}
}
}
// Not too close to the escape pod!
if (distance2d(x,y, REGION_WIDTH/2, REGION_HEIGHT/2) < 15.0F) ok = false;
if (!ok) ++tries;
if (tries > 50) ok = true;
}
// Spawn the hut
if (tries < 51) {
std::cout << "Building passing civ_id: " << civ_id << "\n";
const int n_spawn = build_building(*building, x, y, z, region, spawn_points, active, civ_id);
i += (n_spawn -1);
}
}
}
double MaximumSpread(double x[], double y[], int size){
int i;
int ans=0;
if(i<=1) return 0;
for(i=0;i<size;i++){
double distance=distance2d(x[i], y[i]);
if(distance>ans){
ans=distance;
}
}
return ans;
}
void HUD_Lua_Class::add_entity_blip(short mtype, short intensity, short x, short y)
{
world_point2d origin, target;
origin.x = origin.y = 0;
target.x = x;
target.y = y;
blip_info info;
info.mtype = mtype;
info.intensity = intensity;
info.distance = distance2d(&origin, &target);
info.direction = arctangent(x, y);
m_blips.push_back(info);
}
double LocalPlanner::costFunction(int e, int z)
{
double cost;
int goal_z = floor(atan2(goal.x-pose.x,goal.y-pose.y)*180.0/PI); //azimuthal angle
int goal_e = floor(atan2(goal.z-pose.z,sqrt((goal.x-pose.x)*(goal.x-pose.x)+(goal.y-pose.y)*(goal.y-pose.y)))*180.0/PI);//elevation angle
waypt = getWaypointFromAngle(e,z);
geometry_msgs::Point p;
p.x = waypt.vector.x;
p.y = waypt.vector.y;
p.z = waypt.vector.z;
if(velocity_x > 1.4 && braking_param )
{
//cost = 2*distance2d(p,goal) + 8*(waypt.vector.z-pose.z); //working till 0.5
double obs_dist = distance2d(p,obs);
double energy_waypoint = 0.5*1.56*velocity_x*velocity_x + 1.56*9.81*waypt.vector.z;
double energy_threshold = 0.5*1.56*3*3 + 1.56*9.81*goal.z;
double energy_diff = energy_waypoint-energy_threshold;
double ke_diff = 0.5*1.56*velocity_x*velocity_x - 0.5*1.56*1.4*1.4;
double pe_diff = 1.56*9.81*pose.z - 1.56*9.81*goal.z;
//cost = -1*x_brake_cost_param*distance2d(p,obs) + z_brake_cost_param*(waypt.vector.z-pose.z);
// if(energy_diff>=0)
// cost = 1*x_brake_cost_param*(1/obs_dist) + -1*z_brake_cost_param*energy_diff;
// if(energy_diff<=0)
// cost = 1*x_brake_cost_param*(1/obs_dist) + z_brake_cost_param*energy_diff;
cost = (1/obs_dist) + x_brake_cost_param*ke_diff*(waypt.vector.x-pose.x) + z_brake_cost_param*pe_diff*(waypt.vector.z-pose.z);
}
else
{
if(velocity_x < 1.4)
first_brake = false;
if(e>-50)
{
cost = goal_cost_param*sqrt((goal_e-e)*(goal_e-e)+(goal_z-z)*(goal_z-z)) + smooth_cost_param*sqrt((p1.x-e)*(p1.x-e)+(p1.y-z)*(p1.y-z)) ; //Best working cost function
}
else
cost = 10000;
}
return cost;
}
void scenarioWaves::makeScenario(QList< QList<LedState> > * scenario, QList<int> *delay)
{
int x = 0;
int y = 0;
int z = 0;
int red = qrand()%255;
int green = qrand()%255;
int blue = qrand()%255;
float distance, height, ripple_interval;
int i;
for (i=0;i<2000;i++)
{
QList<LedState> picture;
for (x=0;x<m_xSize;x++)
{
for (z=0;z<m_ySize;z++)
{
distance = distance2d((float)m_ySize/2,(float)m_ySize/2,x,z)/9.899495*m_ySize;
ripple_interval =1.3;
height = (float)(m_zSize/2)+sin(distance/ripple_interval+(float) i/50)*(float)(m_zSize/2);
LedState led = {x,(int)height, z,red,green,blue};
picture.append(led);
}
}
scenario->append(picture);
delay->append(3);
picture.clear();
}
emit doneCreating();
}
bool LocalPlanner::checkForCollision()
{
bool avoid = false;
geometry_msgs::Point temp;
geometry_msgs::Point p;
p.x = waypt.vector.x;
p.y = waypt.vector.y;
p.z = waypt.vector.z;
pcl::PointCloud<pcl::PointXYZ>::iterator it;
for( it = final_cloud.begin(); it != final_cloud.end(); ++it)
{
temp.x = it->x;
temp.y = it->y;
temp.z = it->z;
if(distance2d(p,temp)< 0.5 && init != 0)
{
avoid = true;
break;
}
}
return avoid;
}
/**
* The goal is to return the best sample point that we can split this
* edge on. Returns the index of said sample point.
*/
int Geom_2D::get_sample_point(Mesh::EdgeHandle &ehandle){
/*
* The goal is to convert these coordinates into 2 dimensions
* and do the necessary calculations there. The position of the
* translated points is arbitrary, we shall merely calculate a
* distance then use a unit vector to find the relevant sample point
*/
Mesh::HalfedgeHandle heh = mesh->halfedge_handle(ehandle, 0);
//starting point. We position our edge along the x-axis.
vector<Point_2D> p;
p.push_back(Point_2D(0.0, 0.0, 0.0)); //0
p.push_back(Point_2D(mesh->calc_edge_length(heh), 0.0, 0.0)); //1
p.push_back(get_2D_point(heh, p[0], p[1])); //2
Mesh::HalfedgeLoopIter heIt;
heIt = mesh->hl_begin(heh);
heIt ++;
p.push_back(get_2D_point(mesh->opposite_halfedge_handle(*heIt), p[2], p[1])); //3
heIt ++;
p.push_back(get_2D_point(mesh->opposite_halfedge_handle(*heIt), p[0], p[2])); //4
Mesh::HalfedgeHandle heh2 = mesh->opposite_halfedge_handle(heh);
p.push_back(get_2D_point(heh2, p[1], p[0])); //5
heIt = mesh->hl_begin(heh2);
heIt ++;
p.push_back(get_2D_point(mesh->opposite_halfedge_handle(*heIt), p[5], p[0])); //6
heIt ++;
p.push_back(get_2D_point(mesh->opposite_halfedge_handle(*heIt), p[1], p[5])); //7
/*
* We aim for simplicity and accuracy over speed in this section.
* cc and r contain the circumcenter and the radius of the circumcircles
* of the provided triangles.
*/
double cc[2*6], r[6];
get_circumcircle(p[0], p[2], p[5], &cc[0], r[0]);
get_circumcircle(p[1], p[2], p[5], &cc[2], r[1]);
get_circumcircle(p[1], p[2], p[3], &cc[4], r[2]);
get_circumcircle(p[1], p[7], p[5], &cc[6], r[3]);
get_circumcircle(p[0], p[2], p[4], &cc[8], r[4]);
get_circumcircle(p[0], p[6], p[5], &cc[10], r[5]);
vector<Mesh::Point>* samps;
samps = &(mesh->property(*samples, ehandle));
double score;
Point_2D p2d(0.0,0.0,0.0);
int maxScore = 0;
int maxIndex = 0;
if (samps->size()==0){
cout<<"*************************"<<endl<<"0 samples"<<endl;
output_point(ehandle);
cout<<"*************************"<<endl;
//this should never happen....but it does.
return -1;
}
for (int i = 0; i < samps->size(); i++){
//first two circles if the sample points are in both we
//score 5
//void Geom_2D::mesh_to_plane(Mesh::HalfedgeHandle heh, Mesh::Point &samp, Point_2D &p0, Point_2D &p1, Point_2D &dest){
score = 0;
mesh_to_plane(heh, (*samps)[i], (p[0]), (p[1]), p2d);
if (score_type == 1){
if (distance2d(cc, p2d)<r[0]){
score += 1.5;
}
if (distance2d(cc+2, p2d)<r[1]){
score += 1.5;
}
}else if (score_type == 0){
if (distance2d(cc, p2d)<r[0]){
score ++;
}
if (distance2d(cc+2, p2d)<r[1]){
score ++;
}
}else if (score_type == 2){
if ((distance2d(cc, p2d)<r[0])&&(distance2d(cc+2, p2d)<r[1])){
score += 5;
}
}
for (int j = 2; j < 6; j++){
if (distance2d(cc+(2*j), p2d)>r[j]){
score++;
}
}
if (score>maxScore){
maxScore = score;
maxIndex = i;
}
}
//cout<<"max Score: "<<maxScore<<endl;
//if ((maxIndex == 0)||(maxIndex == samps->size()-1)) {
if(samps->size()==0){
cout<<"*************************"<<endl;
cout<<"******************index: "<<maxIndex<<endl;
cout<<"*************************"<<endl;
cout<<"samples size: "<<samps->size()<<endl;
cout<<" is flippable "<< mesh->property(*is_flippable, ehandle)<<endl;
cout<<" is Delaunay "<< mesh->property(*is_NDE, ehandle)<<endl;
output_point(ehandle);
cout<<"length: "<<mesh->calc_edge_length(ehandle)<<endl;
cout<<"max score "<<maxScore<<endl;
}
return maxIndex;
}
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))));
}