void GraphManager::toggleMapping(bool mappingOn){
QMutexLocker locker(&optimizer_mutex_);
ROS_INFO_COND(mappingOn, "Switching mapping back on");
ROS_INFO_COND(!mappingOn, "Switching mapping off: Localization continues");
localization_only_ = !mappingOn;
ROS_ERROR("Implementation of localization only broken");
#ifdef DO_FEATURE_OPTIMIZATION
optimizer_->setFixed(landmark_vertices, localization_only_);
#endif
optimizer_->setFixed(camera_vertices, localization_only_);
}
void TaskEventDetector::publish(const task_recorder2_msgs::DataSampleLabel& data_label)
{
SafetyLight_msgs::SetColor set_color;
set_color.r = 255 * (1-data_label.binary_label.label);
set_color.g = 255 * data_label.binary_label.label;
set_color.b = 0;
safety_light_publisher_.publish(set_color);
ROS_INFO_COND(data_label.binary_label.label == task_recorder2_msgs::BinaryLabel::FAILED,
"Classification result is FAILED (%.2f).", data_label.cost_label.cost);
ROS_INFO_COND(data_label.binary_label.label == task_recorder2_msgs::BinaryLabel::SUCCEEDED,
"Classification result is SUCCEEDED (%.2f).", data_label.cost_label.cost);
}
PositionSensorHandler::PositionSensorHandler(ssf_core::Measurements* meas) :
MeasurementHandler(meas)
{
ros::NodeHandle pnh("~");
pnh.param("measurement_world_sensor", measurement_world_sensor_, true);
pnh.param("use_fixed_covariance", use_fixed_covariance_, false);
ROS_INFO_COND(measurement_world_sensor_, "interpreting measurement as sensor w.r.t. world");
ROS_INFO_COND(!measurement_world_sensor_, "interpreting measurement as world w.r.t. sensor (e.g. ethzasl_ptam)");
ROS_INFO_COND(use_fixed_covariance_, "using fixed covariance");
ROS_INFO_COND(!use_fixed_covariance_, "using covariance from sensor");
subscribe();
}
void CLegGrid::makeStates()
{
/*
* measurement = recieved leg detector data
* control command = leg velocity (-robot velocity)
* last state = last estimated state
*/
clearStates();
ROS_INFO_COND(DEBUG,"size of new leg msgs: %lu", grid_->polar_array.current.size());
if(diff_time_.toSec() < 1.0)
{
meas_.assign(grid_->polar_array.current.begin(), grid_->polar_array.current.end());
if(!grid_->polar_array.current.empty())
grid_->polar_array.current.clear();
}
match_meas_.resize(meas_.size(), false);
addLastStates();
addMeasurements();
filterStates();
ROS_ASSERT(cmeas_.size() == cstate_.size());
}
void CHumanGrid::encoderCallBack(const nav_msgs::OdometryConstPtr& msg)
{
velocity_.angular = - msg->twist.twist.angular.z;
velocity_.linear = - sqrt(msg->twist.twist.linear.x * msg->twist.twist.linear.x +
msg->twist.twist.linear.y * msg->twist.twist.linear.y);
velocity_.lin.x = - msg->twist.twist.linear.x;
velocity_.lin.y = - msg->twist.twist.linear.y;
ROS_INFO_COND(DEBUG, "linear: %0.4f angular: %0.4f", velocity_.linear, velocity_.angular);
}
void CLegGrid::keepLastLegs()
{
PolarPose p;
if(! grid_->polar_array.current.empty()) grid_->polar_array.current.clear();
for(size_t i = 0; i < grid_->polar_array.past.size() ; i++)
{
ROS_INFO_COND(DEBUG, "Keeping last heard sound source.");
p.range = grid_->polar_array.past.at(i).range + velocity_.linear * diff_time_.toSec();
p.angle = grid_->polar_array.past.at(i).angle + velocity_.angular * diff_time_.toSec();
grid_->polar_array.current.push_back(p);
}
}
void CLegGrid::spin()
{
if(!grid_->polar_array.predicted.empty()) grid_->polar_array.predicted.clear();
ROS_INFO_COND(DEBUG,"--- spin ---");
makeStates();
updateKF();
grid_->updateGrid(1);
publishPredictedLegs();
publishProbability();
publishOccupancyGrid();
grid_->projectGrid();
publishProjection();
}
void EmotionGenerator::getEmotionDesireRelation(std::string desireType)
{
XmlRpc::XmlRpcValue emotionParam;
std::string namespaceNode = n_->getNamespace().substr(1,n_->getNamespace().length()-1);
std::string paramServerDesire = namespaceNode + "/" + nodeName_ + "/" + desireType;
n_->getParam(paramServerDesire, emotionParam);
// ROS_INFO("Emo gen - param emotion desire relation :%s,%s,%s = %s", n_->getNamespace().c_str() ,
// nodeName_.c_str(),
// desireType.c_str(),
// paramServerDesire.c_str() );
std::map<std::string,double> mapEmotion;
ROS_INFO_COND(emotionParam.begin() != emotionParam.end() , "new desire %s :",desireType.c_str());
for (std::map<std::string, XmlRpc::XmlRpcValue>::iterator it = emotionParam.begin(); it != emotionParam.end(); it++)
{
double emotionFactor = 0;
if((*it).second.getType() == XmlRpc::XmlRpcValue::TypeInt)
{
int value = static_cast<int>((*it).second);
ROS_INFO(" %s %i",(*it).first.c_str(), value);
emotionFactor = (double)value;
}
else if ((*it).second.getType() == XmlRpc::XmlRpcValue::TypeDouble)
{
emotionFactor = static_cast<double>((*it).second);
ROS_INFO(" %s %5.2f",(*it).first.c_str(), emotionFactor);
}
mapEmotion[(*it).first] = emotionFactor;
if(emotionIntensities.count((*it).first) == 0)
{
//initialize this emotion
emotionIntensities[(*it).first] = 0;
}
}
if(mapEmotion.size() > 0)
emotionMatrix[desireType] = mapEmotion;
}
void RosUi::toggleCloudStorage(bool storage) {
ParameterServer::instance()->set("store_pointclouds", storage);
ROS_INFO_COND(storage, "Point clouds will be stored");
ROS_INFO_COND(!storage, "Point clouds will not be stored");
}
void CHumanGrid::transitState()
{
ros::Duration dt = ros::Time::now() - state_time_;
ROS_INFO_COND(STATE_DEBUG, "time duration: %0.4f", dt.toSec());
float eps = 0.01; // TODO: Make eps a param
if (hp_.point.z - probability_threshold_ > eps)
{
ROS_INFO_COND(STATE_DEBUG, "two cues: 1");
if (hp_.point.z - tracked_hp_.point.z > -0.05)
{
ROS_INFO_COND(STATE_DEBUG, "start new state: 3");
newState();
}
else
{
ROS_INFO_COND(STATE_DEBUG, "should I track last state?: 4");
if (dt.toSec() < state_time_threshold_)
{
ROS_INFO_COND(STATE_DEBUG, "Yes! track last state: 5");
predictLastHighestPoint();
hp_.point = tracked_hp_.point;
}
else
{
ROS_INFO_COND(STATE_DEBUG, "NO! reset everything: 6");
resetState();
}
}
}
else
{
ROS_INFO_COND(STATE_DEBUG, "one cue: 2");
if (tracked_hp_.point.z - probability_threshold_ > -1e-2)
{
ROS_INFO_COND(STATE_DEBUG, "should i still track last state? : 6");
if (dt.toSec() < state_time_threshold_)
{
ROS_INFO_COND(STATE_DEBUG, "Yes! track last state: 5");
predictLastHighestPoint();
hp_.point = tracked_hp_.point;
}
else
{
ROS_INFO_COND(STATE_DEBUG, "NO! reset everything: 6");
resetState();
}
}
else
{
ROS_INFO_COND(STATE_DEBUG, "reset state :7");
resetState();
}
}
}
void CHumanGrid::integrateProbabilities()
{
ros::Time now = ros::Time::now();
// float maxw = std::max(std::max(leg_max_, sound_max_), torso_max_);
// lw_ = (leg_max_ > 0.0) ? (maxw / leg_max_) : 1.0;
// sw_ = (sound_max_ > 0.0) ? (maxw / sound_max_) : 1.0;
// tw_ = (torso_max_ > 0.0) ? (maxw / torso_max_) : 1.0;
lw_ = sw_ = tw_ = 1.0; //TODO: REMOVE THESE PARAMS
float num = 0.0;
float denum = (lw_ * leg_weight_) + (sw_ * sound_weight_) + (tw_ * torso_weight_); //
std::vector<float> temp;
temp.resize(prob_.poses.size());
float max = -1000;
for (size_t i = 0; i < grid_->grid_size; i++)
{
num = lw_ * leg_weight_ * leg_prob_.poses.at(i).position.z +
sw_ * sound_weight_ * sound_prob_.poses.at(i).position.z +
tw_ * torso_weight_ * torso_prob_.poses.at(i).position.z;
prob_.poses.at(i).position.z = num / denum;
temp.at(i) = grid_->posterior.at(i) = num / denum;
max = std::max(max, temp.at(i));
}
for (size_t i = 0; i < grid_->grid_size; i++)
{
occupancy_grid_.data.at(i) = (int) 100 * (temp.at(i)) / max;
}
occupancy_grid_.header.stamp = now;
human_grid_pub_.publish(occupancy_grid_);
ROS_INFO_COND(DEBUG, "max probability: %.4f ", max);
// Using Local Maxima
// grid_->updateLocalMaximas();
// hp_.point = grid_->highest_prob_point.point;
//Use highest Point
std::vector<float>::iterator it = std::max_element(temp.begin(), temp.end());
hp_.point = prob_.poses.at(it - temp.begin()).position;
transitState();
last_time_ = now;
hp_.header.stamp = now;
highest_point_pub_.publish(hp_);
printFusedFeatures();
publishLocalMaxima();
grid_->projectGrid();
publishProjection();
}
// The given `check' function must be called every iteration.
// It may call setPreempted() or perform other tasks.
bool BaseStrategy::execute(std::function<bool()> check)
{
ros::Rate r(2); // Hz
while (node_.ok() && check() && !isPreempted() && !finished_)
{
callback_queue_.callAvailable();
// TODO: check map age.
// TODO: getNumPublishers here sometimes segfaults on
// succeeded/preempted, for no apparent reason.
if (slam_map_subscriber_.getNumPublishers() == 0)
{
// SLAM is not running.
ROS_INFO("Waiting for SLAM to start publishing a map...");
}
else if (map_ && pose_initialized_)
{
if (obstacle_map_publisher_.getNumSubscribers() > 0)
{
obstacle_map_publisher_.publish(createOccupancyGrid(*map_));
}
if (!map_ready_)
{
const int Rfree = std::ceil(initial_free_radius_ / map_->resolution);
freeRobotPose(*map_, map_->getIdxForPosition(pose_.position), Rfree);
// TODO: This is too conservative. "!= -1" should be used instead, but
// for now it's like this to handle the point of a map where the
// only frontier is the robot position.
if (map_updated_ && map_->isBoxKnown(pose_.position, safety_radius_) == 1)
{
map_ready_ = true;
}
else
{
ROS_INFO_COND(map_updated_, "Position is outside map. Initialization manoeuvres...");
executeInitStep(map_updated_);
}
}
if (map_ready_)
{
if (map_updated_)
{
executeStep();
}
else
{
executeControlStep();
}
}
map_updated_ = false;
}
else
{
if (map_updated_)
{
ROS_INFO("Received map is empty. Initialization manoeuvres...");
}
executeInitStep(map_updated_);
}
r.sleep();
}
return finished_;
}
std::vector<GraspScored> Reaching::selectFeasibleGrasps(const agile_grasp::Grasps& grasps_in)
{
std::vector<GraspScored> grasps_selected;
// evaluate the reachability of each grasp
for (int i = 0; i < grasps_in.grasps.size(); i++)
{
const agile_grasp::Grasp& grasp = grasps_in.grasps[i];
// check whether grasp lies within the workspace of the robot arm
ROS_INFO_COND(params_.is_printing_, "Checking if grasp %i, position (%1.2f, %1.2f, %1.2f), can be reached: ", i,
grasp.center.x, grasp.center.y, grasp.center.z);
if (!isInWorkspace(grasp.surface_center.x, grasp.surface_center.y, grasp.surface_center.z))
{
ROS_INFO_COND(params_.is_printing_, " NOT OK!");
continue;
}
ROS_INFO_COND(params_.is_printing_, " OK");
// avoid objects that are smaller/larger than the minimum/maximum robot hand aperture
ROS_INFO_COND(params_.is_printing_, "Checking aperture: ");
if (grasp.width.data < params_.min_aperture_ || grasp.width.data > params_.max_aperture_)
{
ROS_INFO_COND(params_.is_printing_, "too small/large for the hand (min, max): %.4f (%.4f, %.4f)!",
grasp.width.data, params_.min_aperture_, params_.max_aperture_);
continue;
}
ROS_INFO_COND(params_.is_printing_, " OK");
GraspEigen grasp_eigen(grasp);
// generate additional grasps
Eigen::VectorXd theta;
if (params_.num_additional_grasps_ > 0)
{
theta = Eigen::VectorXd::LinSpaced(1 + params_.num_additional_grasps_, -15.0, 15.0);
}
else
{
theta.resize(1);
theta << 0.0;
}
// check all grasps for reachability
for (int j = 0; j < theta.size(); j++)
{
ROS_INFO_COND(params_.is_printing_, "j: %i", j);
// calculate approach vector and hand axis for the new grasp
GraspEigen grasp_eigen_rot = rotateGrasp(grasp_eigen, theta[j]);
// create a grasp for each hand orientation and check whether they are reachable by the IK and collision-free
std::vector<tf::Quaternion> quats = calculateHandOrientations(grasp_eigen_rot);
bool is_collision_free = false;
for (int k = 0; k < quats.size(); k++)
{
ROS_INFO_COND(params_.is_printing_, "k: %i", k);
// create grasp pose
geometry_msgs::PoseStamped grasp_pose = createGraspPose(grasp_eigen_rot, quats[k], theta[j]);
// try to solve IK
ROS_INFO_COND(params_.is_printing_, " Solving IK: ");
double tik0 = omp_get_wtime();
IKSolution ik_solution = solveIK(grasp_pose);
ROS_INFO_COND(params_.is_printing_, " IK runtime: %.2f", omp_get_wtime() - tik0);
if (!ik_solution.success_) // IK fails
{
ROS_INFO_COND(params_.is_printing_, "IK failed for grasp %i, approach %i, orientation %i!\n", i, j, k);
continue;
}
ROS_INFO_COND(params_.is_printing_, " OK");
// check collisions (only required for one orientation/quaternion)
ROS_INFO_COND(params_.is_printing_, " Checking collisions: ");
if (!is_collision_free)
{
double tcoll0 = omp_get_wtime();
is_collision_free = isCollisionFree(grasp_pose, grasp_eigen_rot.approach_);
ROS_INFO_COND(params_.is_printing_, " Collision checker runtime: %.2f", omp_get_wtime() - tcoll0);
if (!is_collision_free)
{
ROS_INFO_COND(params_.is_printing_, "Grasp %i, approach %i, orientation %i collides with point cloud!\n", i,
j, k);
continue;
}
}
ROS_INFO_COND(params_.is_printing_, " OK");
if (params_.is_printing_)
{
std::cout << "IK solution: ";
for(int t=0; t < ik_solution.joint_positions_.size(); t++)
std::cout << ik_solution.joint_positions_[t] << " ";
std::cout << std::endl;
}
// create grasp based on inverse kinematics solution
GraspScored grasp_scored(i, grasp_pose, grasp_eigen_rot.approach_, grasp.width.data, ik_solution.joint_positions_, 0.0);
grasps_selected.push_back(grasp_scored);
}
}
}
return grasps_selected;
}
