void PlaneRegistration::estimate_during_cutting(){
fk_solver_->JntToCart( jnt_pos_, tip_frame_ );
Eigen::Vector3d startPoint_temp( startPoint );
// reset start point every 5 seconds to capture the most recent motion
if ( ros::Time::now().toSec() - last_reset_start_point_time > 5 ){
startPoint_temp = curr_point_history.col(0);
last_reset_start_point_time = ros::Time::now().toSec();
}
if ( ptsloc == 1 ){
Eigen::Vector3d curr_point( tip_frame_.p[0], tip_frame_.p[1], tip_frame_.p[2] );
trajectory_unit_vector = estimate_trj_unit_vector( startPoint_temp,
curr_point,
trajectory_unit_vector_history );
trajectory_unit_vector_last = trajectory_unit_vector;
}
if ( ptsloc == 0 ){
trajectory_unit_vector = trajectory_unit_vector_last;
}
trajectory_unit_vector_msg.x = trajectory_unit_vector(0);
trajectory_unit_vector_msg.y = trajectory_unit_vector(1);
trajectory_unit_vector_msg.z = trajectory_unit_vector(2);
pub_traj_unit_vector_.publish( trajectory_unit_vector_msg );
}
void PlaneRegistration::local_probing(){
fk_solver_->JntToCart( jnt_pos_, tip_frame_ );
Eigen::Vector3d start_point( startPoint(0), startPoint(1), startPoint(2) );
Eigen::Vector3d curr_point( tip_frame_.p[0], tip_frame_.p[1], tip_frame_.p[2] );
// cutter moving in the first straight line, estimate the first vector
if ( ptsloc == 1 ){
plane_vec_1 = estimate_trj_unit_vector( start_point,
curr_point,
unit_vector_history_1 );
ptsloclast = 1;
}
// resetting start point when moving from direction 1 to direction 2
if ( ptsloc == -1 ){
reset_start_point = 1;
}
// cutter moving in another direction, estimate the second vector
if ( ptsloc == 2 ){
plane_vec_2 = estimate_trj_unit_vector( start_point,
curr_point,
unit_vector_history_2 );
ptsloclast = 2;
}
// finished collecting two vectors on plane, calculate the normal
if ( ptsloc == 3 ){
Eigen::Vector3d normal;
normal = plane_vec_1.cross( plane_vec_2 );
// make sure the reported normal makes an angle < 90 degrees with the cutter z axis
Eigen::Vector3d tipZ( tip_frame_.M.UnitZ()[0],
tip_frame_.M.UnitZ()[1],
tip_frame_.M.UnitZ()[2] );
if ( normal.dot( tipZ ) < 0 ){
normal = -normal;
}
Eigen::Vector3d unit_normal = normal / normal.norm();
std::cout << "est. Normal: "<< std::endl;
std::cout << unit_normal <<std::endl;
local_probing_est.x = unit_normal(0);
local_probing_est.y = unit_normal(1);
local_probing_est.z = unit_normal(2);
pub_local_probing_normal_est_.publish( local_probing_est );
}
}
bool
SSLPathPlanner::doPlanForRobot (const int& id/*, bool& is_stop*/)
{
// is_send_plan = true;
tmp_robot_id_queried = id;
State* tmp_start = manifold->allocState ();
tmp_start->as<RealVectorStateManifold::StateType> ()->values[0] = team_state_[id].pose.x;
tmp_start->as<RealVectorStateManifold::StateType> ()->values[1] = team_state_[id].pose.y;
PathGeometric direct_path (planner_setup->getSpaceInformation ());
direct_path.states.push_back (manifold->allocState ());
manifold->copyState (direct_path.states[0], tmp_start);
State* tmp_goal = manifold->allocState ();
tmp_goal->as<RealVectorStateManifold::StateType> ()->values[0] = pose_control.pose[id].pose.x;
tmp_goal->as<RealVectorStateManifold::StateType> ()->values[1] = pose_control.pose[id].pose.y;
// std::cout<<pose_control.pose[id].pose.x<<"\t"<<pose_control.pose[id].pose.y<<std::endl;
direct_path.states.push_back (manifold->allocState ());
manifold->copyState (direct_path.states[1], tmp_goal);
Point_2 p_start (team_state_[id].pose.x, team_state_[id].pose.y);
Point_2 p_goal (pose_control.pose[id].pose.x, pose_control.pose[id].pose.y);
//too close to the target point, no need for planning, just take it as a next_target_pose
if (sqrt (getSquaredDistance (p_start, p_goal)) < VERY_CRITICAL_DIST)
{
next_target_poses[id].x = pose_control.pose[id].pose.x;
next_target_poses[id].y = pose_control.pose[id].pose.y;
next_target_poses[id].theta = pose_control.pose[id].pose.theta;
return true;
}
// manifold->printState (direct_path.states[0], std::cout);
// manifold->printState (direct_path.states[1], std::cout);
//direct path is available, no need for planning
// if (direct_path.check ())
if (!doesIntersectObstacles (id, p_start, p_goal))
{
// std::cout << "direct path is AVAILABLE for robot " << id << std::endl;
if (solution_data_for_robots[id].size () > direct_path.states.size ())
{
for (uint32_t i = direct_path.states.size (); i < solution_data_for_robots[id].size (); i++)
manifold->freeState (solution_data_for_robots[id][i]);
solution_data_for_robots[id].resize (direct_path.states.size ());
}
else if (solution_data_for_robots[id].size () < direct_path.states.size ())
{
for (uint32_t i = solution_data_for_robots[id].size (); i < direct_path.states.size (); i++)
solution_data_for_robots[id].push_back (manifold->allocState ());
}
for (uint32_t i = 0; i < direct_path.states.size (); i++)
manifold->copyState (solution_data_for_robots[id][i], direct_path.states[i]);
manifold->freeState (tmp_start);
manifold->freeState (tmp_goal);
next_target_poses[id].x = pose_control.pose[id].pose.x;
next_target_poses[id].y = pose_control.pose[id].pose.y;
next_target_poses[id].theta = pose_control.pose[id].pose.theta;
// std::cout<<next_target_poses[id].x<<"\t"<<next_target_poses[id].y<<std::endl;
return true;
}
manifold->freeState (tmp_start);
manifold->freeState (tmp_goal);
// std::cout << "direct path is NOT AVAILABLE for robot " << id << ", doing planning" << std::endl;
//direct path is not available, DO PLAN if the earlier plan is invalidated!
//earlier plan is still valid
if (checkPlanForRobot (id))
return true;
else if (is_plan_done[id])
return false;
//earlier plan is not valid anymore, replan
planner_setup->clear ();
planner_setup->clearStartStates ();
ScopedState<RealVectorStateManifold> start_state (manifold);
ScopedState<RealVectorStateManifold> goal_state (manifold);
start_state->values[0] = team_state_[id].pose.x;
start_state->values[1] = team_state_[id].pose.y;
goal_state->values[0] = pose_control.pose[id].pose.x;
goal_state->values[1] = pose_control.pose[id].pose.y;
planner_setup->setStartAndGoalStates (start_state, goal_state);
planner_setup->getProblemDefinition ()->fixInvalidInputStates (ssl::config::ROBOT_BOUNDING_RADIUS / 2.0,
ssl::config::ROBOT_BOUNDING_RADIUS / 2.0, 100);
bool solved = planner_setup->solve (0.100);//100msec
if (solved)
{
planner_setup->simplifySolution ();
PathGeometric* path;
path = &planner_setup->getSolutionPath ();
// PathSimplifier p (planner_setup->getSpaceInformation ());
// p.reduceVertices (*path, 1000);
if (solution_data_for_robots[id].size () < path->states.size ())
{
for (unsigned int i = solution_data_for_robots[id].size (); i < path->states.size (); i++)
solution_data_for_robots[id].push_back (manifold->allocState ());
}
else if (solution_data_for_robots[id].size () > path->states.size ())
{
for (unsigned int i = path->states.size (); i < solution_data_for_robots[id].size (); i++)
manifold->freeState (solution_data_for_robots[id][i]);
//drop last elements that are already being freed
solution_data_for_robots[id].resize (path->states.size ());
}
for (unsigned int i = 0; i < path->states.size (); i++)
manifold->copyState (solution_data_for_robots[id][i], path->states[i]);
//leader-follower approach based segment enlargement
//pick next location
Point_2 curr_point (path->states[0]->as<RealVectorStateManifold::StateType> ()->values[0], path->states[0]->as<
RealVectorStateManifold::StateType> ()->values[1]);
for (uint32_t i = 1; i < path->states.size (); i++)
{
Point_2 next_point (path->states[i]->as<RealVectorStateManifold::StateType> ()->values[0], path->states[i]->as<
RealVectorStateManifold::StateType> ()->values[1]);
if (!doesIntersectObstacles (id, curr_point, next_point))
{
next_target_poses[id].x = path->states[i]->as<RealVectorStateManifold::StateType> ()->values[0];
next_target_poses[id].y = path->states[i]->as<RealVectorStateManifold::StateType> ()->values[1];
next_target_poses[id].theta = pose_control.pose[id].pose.theta;
}
else
break;
}
// next_target_poses[id].x = path->states[1]->as<RealVectorStateManifold::StateType> ()->values[0];
// next_target_poses[id].y = path->states[1]->as<RealVectorStateManifold::StateType> ()->values[1];
// planner_data_for_robots[id].clear ();
// planner_data_for_robots[id] = planner_setup->getPlannerData ();
}
return solved;
}
