/**
* @return either one of control_msgs::FollowJointTrajectoryResult, or PREEMPT_REQUESTED
*/
int JointTrajectoryActionController::executeCommon(const trajectory_msgs::JointTrajectory &trajectory,
boost::function<bool()> isPreemptRequested)
{
if (!setsEqual(joints_, trajectory.joint_names))
{
ROS_ERROR("Joints on incoming goal don't match our joints");
for (size_t i = 0; i < trajectory.joint_names.size(); i++)
{
ROS_INFO(" incoming joint %d: %s", (int)i, trajectory.joint_names[i].c_str());
}
for (size_t i = 0; i < joints_.size(); i++)
{
ROS_INFO(" our joint %d: %s", (int)i, joints_[i].c_str());
}
return control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
}
if (isPreemptRequested())
{
ROS_WARN("New action goal already seems to have been canceled!");
return PREEMPT_REQUESTED;
}
// make sure the katana is stopped
reset_trajectory_and_stop();
// ------ If requested, performs a stop
if (trajectory.points.empty())
{
// reset_trajectory_and_stop();
return control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
}
// calculate new trajectory
boost::shared_ptr<SpecifiedTrajectory> new_traj = calculateTrajectory(trajectory);
if (!new_traj)
{
ROS_ERROR("Could not calculate new trajectory, aborting");
return control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
}
if (!validTrajectory(*new_traj))
{
ROS_ERROR("Computed trajectory did not fulfill all constraints!");
return control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
}
current_trajectory_ = new_traj;
// sleep until 0.5 seconds before scheduled start
ROS_DEBUG_COND(
trajectory.header.stamp > ros::Time::now(),
"Sleeping for %f seconds before start of trajectory", (trajectory.header.stamp - ros::Time::now()).toSec());
ros::Rate rate(10);
while ((trajectory.header.stamp - ros::Time::now()).toSec() > 0.5)
{
if (isPreemptRequested() || !ros::ok())
{
ROS_WARN("Goal canceled by client while waiting until scheduled start, aborting!");
return PREEMPT_REQUESTED;
}
rate.sleep();
}
ROS_INFO("Sending trajectory to Katana arm...");
bool success = katana_->executeTrajectory(new_traj, isPreemptRequested);
if (!success)
{
ROS_ERROR("Problem while transferring trajectory to Katana arm, aborting");
return control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
}
ROS_INFO("Waiting until goal reached...");
ros::Rate goalWait(10);
while (ros::ok())
{
// always have to call this before calling someMotorCrashed() or allJointsReady()
katana_->refreshMotorStatus();
if (katana_->someMotorCrashed())
{
ROS_ERROR("Some motor has crashed! Aborting trajectory...");
return control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
}
// all joints are idle
if (katana_->allJointsReady() && allJointsStopped())
{
// // make sure the joint positions are updated before checking for goalReached()
// --> this isn't necessary because refreshEncoders() is periodically called
// by KatanaNode. Leaving it out saves us some Katana bandwidth.
// katana_->refreshEncoders();
if (goalReached())
{
// joints are idle and we are inside goal constraints. yippie!
ROS_INFO("Goal reached.");
return control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
}
else
{
ROS_ERROR("Joints are idle and motors are not crashed, but we did not reach the goal position! WTF?");
return control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED;
}
}
if (isPreemptRequested())
{
ROS_WARN("Goal canceled by client while waiting for trajectory to finish, aborting!");
return PREEMPT_REQUESTED;
}
goalWait.sleep();
}
// this part is only reached when node is shut down
return PREEMPT_REQUESTED;
}
void ExploreFrontier::findFrontiers() {
frontiers_.clear();
const size_t w = costmap_->getSizeInCellsX();
const size_t h = costmap_->getSizeInCellsY();
const size_t size = w * h;
map_.info.width = w;
map_.info.height = h;
map_.data.resize(size);
map_.info.resolution = costmap_->getResolution();
map_.info.origin.position.x = costmap_->getOriginX();
map_.info.origin.position.y = costmap_->getOriginY();
// lock as we are accessing raw underlying map
auto *mutex = costmap_->getMutex();
std::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*mutex);
// Find all frontiers (open cells next to unknown cells).
const unsigned char* map = costmap_->getCharMap();
ROS_DEBUG_COND(!map, "no map available");
for (size_t i = 0; i < size; ++i) {
// //get the world point for the index
// unsigned int mx, my;
// costmap.indexToCells(i, mx, my);
// geometry_msgs::Point p;
// costmap.mapToWorld(mx, my, p.x, p.y);
//
//check if the point has valid potential and is next to unknown space
// bool valid_point = planner_->validPointPotential(p);
bool valid_point = (map[i] < costmap_2d::LETHAL_OBSTACLE);
// ROS_DEBUG_COND(!valid_point, "invalid point %u", map[i]);
if ((valid_point && map) &&
(((i+1 < size) && (map[i+1] == costmap_2d::NO_INFORMATION)) ||
((i-1 < size) && (map[i-1] == costmap_2d::NO_INFORMATION)) ||
((i+w < size) && (map[i+w] == costmap_2d::NO_INFORMATION)) ||
((i-w < size) && (map[i-w] == costmap_2d::NO_INFORMATION))))
{
ROS_DEBUG_THROTTLE(30, "found suitable point");
map_.data[i] = -128;
} else {
map_.data[i] = -127;
}
}
// Clean up frontiers detected on separate rows of the map
for (size_t y = 0, i = h-1; y < w; ++y) {
ROS_DEBUG_THROTTLE(30, "cleaning cell %lu", i);
map_.data[i] = -127;
i += h;
}
// Group adjoining map_ pixels
signed char segment_id = 127;
std::vector<std::vector<FrontierPoint>> segments;
for (size_t i = 0; i < size; i++) {
if (map_.data[i] == -128) {
ROS_DEBUG_THROTTLE(30, "adjoining on %lu", i);
std::vector<size_t> neighbors;
std::vector<FrontierPoint> segment;
neighbors.push_back(i);
// claim all neighbors
while (!neighbors.empty()) {
ROS_DEBUG_THROTTLE(30, "got %lu neighbors", neighbors.size());
size_t idx = neighbors.back();
neighbors.pop_back();
map_.data[idx] = segment_id;
tf::Vector3 tot(0,0,0);
size_t c = 0;
if (idx+1 < size && map[idx+1] == costmap_2d::NO_INFORMATION) {
tot += tf::Vector3(1,0,0);
++c;
}
if (idx-1 < size && map[idx-1] == costmap_2d::NO_INFORMATION) {
tot += tf::Vector3(-1,0,0);
++c;
}
if (idx+w < size && map[idx+w] == costmap_2d::NO_INFORMATION) {
tot += tf::Vector3(0,1,0);
++c;
}
if (idx-w < size && map[idx-w] == costmap_2d::NO_INFORMATION) {
tot += tf::Vector3(0,-1,0);
++c;
}
if(!(c > 0)) {
ROS_ERROR("assertion failed. corrupted costmap?");
ROS_DEBUG("c is %lu", c);
continue;
}
segment.push_back(FrontierPoint(idx, tot / c));
// consider 8 neighborhood
if (idx-1 < size && map_.data[idx-1] == -128)
neighbors.push_back(idx-1);
if (idx+1 < size && map_.data[idx+1] == -128)
neighbors.push_back(idx+1);
if (idx-w < size && map_.data[idx-w] == -128)
neighbors.push_back(idx-w);
if (idx-w+1 < size && map_.data[idx-w+1] == -128)
neighbors.push_back(idx-w+1);
if (idx-w-1 < size && map_.data[idx-w-1] == -128)
neighbors.push_back(idx-w-1);
if (idx+w < size && map_.data[idx+w] == -128)
neighbors.push_back(idx+w);
if (idx+w+1 < size && map_.data[idx+w+1] == -128)
neighbors.push_back(idx+w+1);
if (idx+w-1 < size && map_.data[idx+w-1] == -128)
neighbors.push_back(idx+w-1);
}
segments.push_back(std::move(segment));
segment_id--;
if (segment_id < -127)
break;
}
}
// no longer accessing map
lock.unlock();
int num_segments = 127 - segment_id;
if (num_segments <= 0) {
ROS_DEBUG("#segments is <0, no frontiers found");
return;
}
for (auto& segment : segments) {
Frontier frontier;
size_t segment_size = segment.size();
//we want to make sure that the frontier is big enough for the robot to fit through
if (segment_size * costmap_->getResolution() < costmap_client_->getInscribedRadius()) {
ROS_DEBUG_THROTTLE(30, "some frontiers were small");
continue;
}
float x = 0, y = 0;
tf::Vector3 d(0,0,0);
for (const auto& frontier_point : segment) {
d += frontier_point.d;
size_t idx = frontier_point.idx;
x += (idx % w);
y += (idx / w);
}
d = d / segment_size;
frontier.pose.position.x = map_.info.origin.position.x + map_.info.resolution * (x / segment_size);
frontier.pose.position.y = map_.info.origin.position.y + map_.info.resolution * (y / segment_size);
frontier.pose.position.z = 0.0;
frontier.pose.orientation = tf::createQuaternionMsgFromYaw(atan2(d.y(), d.x()));
frontier.size = segment_size;
frontiers_.push_back(std::move(frontier));
}
}
