collision_detection::CollisionResult
CollisionCheckMoveIt::getCollisionResult(const sensor_msgs::JointState& targetJointsState, bool contacts)
{
//ROS_INFO_STREAM("Checking collision for:\n" << targetJointsState);
planning_scene_monitor::LockedPlanningSceneRO locked_planning_scene(m_planning_scene_monitor);
const robot_state::RobotState& current_state = locked_planning_scene->getCurrentState();
const robot_state::RobotModelConstPtr& robot_model = current_state.getRobotModel();
const std::vector<std::string>& robot_joint_names = robot_model->getJointModelNames();
collision_detection::CollisionResult collision_result;
robot_state::RobotState target_state(current_state);
for (size_t jointIdx = 0; jointIdx < targetJointsState.name.size(); jointIdx++) {
std::string joint_name;
if (m_joint_names_map.size() == 0) {
joint_name = targetJointsState.name[jointIdx];
} else {
joint_name = m_joint_names_map[targetJointsState.name[jointIdx]];
}
// check if joint_name is part of robot_model
if (joint_name.size() == 0
|| std::find(robot_joint_names.begin(), robot_joint_names.end(), joint_name) == robot_joint_names.end()) {
ROS_INFO("Invalid joint name: '%s'. Considering this as collision.", joint_name.c_str());
collision_result.collision = true;
return collision_result;
}
target_state.setJointPositions(joint_name, &targetJointsState.position[jointIdx]);
}
target_state.update();
collision_detection::CollisionRequest collision_request;
collision_request.contacts = contacts;
#ifdef DEBUG_COLLISIONS
collision_request.contacts = true;
#endif
collision_request.max_contacts = 100;
collision_result.clear();
locked_planning_scene->checkCollision(collision_request, collision_result, target_state);
#ifdef DEBUG_COLLISIONS
if (collision_result.collision) {
collision_detection::CollisionResult::ContactMap::const_iterator it;
for(it = collision_result.contacts.begin();
it != collision_result.contacts.end();
++it) {
ROS_INFO("Collision between: %s and %s", it->first.first.c_str(), it->first.second.c_str());
}
}
#endif
return collision_result;
}
bool mouse_drag_listener::process_event(const SDL_Event& event, bool claimed) {
Sint32 pos[2];
Sint16 rel[2];
Uint8 state;
Uint8 button_change_state;
SDLMod mod;
SDLMod mod_change_state;
bool was_capturing = is_capturing_;
if(claimed) {
is_capturing_ = false;
return claimed;
}
switch(event.type) {
case SDL_MOUSEMOTION:
pos[0] = event.motion.x;
pos[1] = event.motion.y;
rel[0] = event.motion.xrel;
rel[1] = event.motion.yrel;
state = event.motion.state;
mod = SDL_GetModState();
button_change_state = 0;
mod_change_state = KMOD_NONE;
break;
case SDL_MOUSEBUTTONDOWN:
case SDL_MOUSEBUTTONUP:
pos[0] = event.button.x;
pos[1] = event.button.y;
rel[0] = 0;
rel[1] = 0;
state = SDL_GetMouseState(NULL,NULL);
mod = SDL_GetModState();
button_change_state =
get_button_change_state(event.button.button)
& target_state_mask();
mod_change_state = KMOD_NONE;
break;
case SDL_KEYDOWN:
case SDL_KEYUP:
state = SDL_GetMouseState(&(pos[0]), &(pos[1]));
rel[0] = 0;
rel[1] = 0;
mod = event.key.keysym.mod;
button_change_state = 0;
mod_change_state = (SDLMod)
(get_mod_change_state(event.key.keysym.sym)
& target_mod_mask());
break;
default:
return claimed;
}
if(!matches_target_state(state)) {
is_capturing_ = false;
return claimed;
} else if(!matches_target_mod(mod)) {
is_capturing_ = false;
return claimed;
}
if(!is_capturing_) {
/* to start a gesture, we must be in the target area (if one is defined)
AND this must be a mouse state change event if buttons are involved
OR this must be a key state change event if keys are involved
*/
if(target_state() != 0) {
/* if we have a target state, this must have been an event
which indicated an unmasked button was pressed */
if(button_change_state == 0) {
return claimed;
}
} else if(target_mod() != 0) {
/* if we don't have a target state, but do have target modifiers,
this must have been an event that indicated an unmasked
modifier was pressed */
if(mod_change_state == 0) {
return claimed;
}
}
if(target_area_enabled() && !in_target_area(pos[0], pos[1])) {
return claimed;
}
}
is_capturing_ = true;
claimed = true;
pos_[0] = pos[0];
pos_[1] = pos[1];
state_ = state;
mod_ = mod;
rel_[0] = rel[0];
rel_[1] = rel[1];
if(was_capturing) {
total_rel_[0] += rel[0];
total_rel_[1] += rel[1];
} else {
total_rel_[0] = rel[0];
total_rel_[1] = rel[1];
start_pos_[0] = pos[0];
start_pos_[1] = pos[1];
}
do_drag();
return claimed;
}
std::pair <std::vector<StateOfCar>, Seed> AStarSeed::findPathToTarget(const cv::Mat& map, const State& start, const State& goal, const int distance_transform, const int debug_current_state, int& status) {
// USE : for guaranteed termination of planner
int no_of_iterations = 0;
int max_iterations = 10000;
node_handle.getParam("local_planner/max_iterations", max_iterations);
fusion_map = map;
map_max_rows = map.rows;
map_max_cols = map.cols;
if (distance_transform == 1) {
distanceTransform();
}
if (debug_current_state) {
image = fusion_map.clone();
}
StateOfCar start_state(start), target_state(goal);
std::map<StateOfCar, open_map_element> open_map;
std::map<StateOfCar, StateOfCar, comparatorMapState> came_from;
SS::PriorityQueue<StateOfCar> open_set;
open_set.push(start_state);
if (start_state.isCloseTo(target_state)) {
status = 1;
return std::make_pair(std::vector<StateOfCar>(), Seed());
} else if (isOnTheObstacle(start_state)) {
std::cout << "Bot is on the Obstacle Map \n";
return std::make_pair(std::vector<StateOfCar>(), Seed());
} else if (isOnTheObstacle(target_state)) {
std::cout << "Target is on the Obstacle Map \n";
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
while (!open_set.empty() && ros::ok()) {
if (no_of_iterations > max_iterations) {
status = open_set.size();
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
StateOfCar current_state = open_set.top();
if (open_map.find(current_state) != open_map.end() && open_map[current_state].membership == CLOSED) {
open_set.pop();
}
current_state = open_set.top();
if (debug_current_state) {
std::cout << "current x : " << current_state.x() << " current y : " << current_state.y() << std::endl;
plotPointInMap(current_state);
cv::imshow("[PLANNER] Map", image);
cvWaitKey(0);
}
// TODO : use closeTo instead of onTarget
if (current_state.isCloseTo(target_state)) {
status = 2;
return reconstructPath(current_state, came_from);
}
open_set.pop();
open_map[current_state].membership = UNASSIGNED;
open_map[current_state].cost = -current_state.gCost();
open_map[current_state].membership = CLOSED;
std::vector<StateOfCar> neighbors_of_current_state = neighborNodesWithSeeds(current_state);
for (unsigned int iterator = 0; iterator < neighbors_of_current_state.size(); iterator++) {
StateOfCar neighbor = neighbors_of_current_state[iterator];
double tentative_gcost_along_followed_path = neighbor.gCost() + current_state.gCost();
double admissible = neighbor.distanceTo(target_state);
double consistent = admissible;
if (!((open_map.find(neighbor) != open_map.end()) &&
(open_map[neighbor].membership == OPEN))) {
came_from[neighbor] = current_state;
neighbor.hCost(consistent);
neighbor.gCost(tentative_gcost_along_followed_path);
neighbor.updateTotalCost();
open_set.push(neighbor);
open_map[neighbor].membership = OPEN;
open_map[neighbor].cost = neighbor.gCost();
}
}
no_of_iterations++;
}
status = 0;
return std::make_pair(std::vector<StateOfCar>(), Seed());
}
