void dynConfigCallback(Config &cfg, uint32_t level) {
boost::mutex::scoped_lock lock(cfg_mutex_);
x_acc_lim_ = cfg.x_acc_lim;
y_acc_lim_ = cfg.y_acc_lim;
yaw_acc_lim_ = cfg.yaw_acc_lim;
interpolate_max_frame_ = cfg.interpolate_max_frame;
if(desired_rate_ != cfg.desired_rate) {
desired_rate_ = cfg.desired_rate;
if (timer_.isValid()) {
ros::Duration d = timerDuration();
timer_.setPeriod(d);
ROS_INFO_STREAM("timer loop rate is changed to " << 1.0 / d.toSec() << "[Hz]");
}
}
}
void callback(octoflex::OctoFlexConfig &config, uint32_t level)
{
checkFullVolume_ = config.checkFullVolume;
simTime_ = config.forwardTime;
numSteps_ = config.numberOfSteps;
length_ = config.length;
width_ = config.width;
height_ = config.height;
originOffsetX_ = config.originOffsetX;
originOffsetY_ = config.originOffsetY;
originOffsetZ_ = config.originOffsetZ;
resolution_ = config.resolution;
rate_ = config.rate;
visPause_ = config.visualizationPause;
loopTimer_.setPeriod(ros::Duration(1.0/rate_));
}
void CurrentActionListCallback(const supervisor_msgs::ActionsList::ConstPtr& msg)
{
try
{
if(!msg->actions.empty())
{
for(int i = 0 ; i < msg->actions.size() ; ++i)
{
for(int j = 0 ; j < msg->actions[i].actors.size() ; ++j)
{
if(msg->actions[i].actors[j] == "HERAKLES_HUMAN1")
{
ROS_INFO("[robot_observer] Action detected");
if(msg->actions[i].focusTarget=="RED_CUBE"){
task_started_=true;
current_action_position_=red_cube_position_;
current_action_=msg->actions[i];
previous_action_=msg->actions[i];
object_position_=current_action_position_;
enable_event_=false;
waiting_timer_.setPeriod(ros::Duration(1.0));
waiting_timer_.start();
task_started_=true;
state_machine_->process_event(humanActing());
}
if(msg->actions[i].focusTarget=="BLACK_CUBE"){
task_started_=true;
current_action_position_=black_cube_position_;
current_action_=msg->actions[i];
previous_action_=msg->actions[i];
object_position_=current_action_position_;
enable_event_=false;
waiting_timer_.setPeriod(ros::Duration(1.0));
waiting_timer_.start();
task_started_=true;
state_machine_->process_event(humanActing());
}
if(msg->actions[i].focusTarget=="BLUE_CUBE"){
task_started_=true;
current_action_position_=blue_cube_position_;
current_action_=msg->actions[i];
previous_action_=msg->actions[i];
enable_event_=false;
waiting_timer_.setPeriod(ros::Duration(1.0));
waiting_timer_.start();
task_started_=true;
state_machine_->process_event(humanActing());
}
if(msg->actions[i].focusTarget=="GREEN_CUBE2"){
task_started_=true;
current_action_position_=green_cube_position_;
current_action_=msg->actions[i];
previous_action_=msg->actions[i];
enable_event_=false;
waiting_timer_.setPeriod(ros::Duration(1.0));
waiting_timer_.start();
task_started_=true;
state_machine_->process_event(humanActing());
}
if(msg->actions[i].focusTarget=="PLACEMAT_RED"){
task_started_=true;
current_action_position_=placemat_position_;
current_action_ =msg->actions[i];
previous_action_=msg->actions[i];
object_position_=current_action_position_;
enable_event_=false;
waiting_timer_.setPeriod(ros::Duration(1.0));
waiting_timer_.start();
task_started_=true;
state_machine_->process_event(humanActing());
}
}
}
}
}
}
catch (HeadManagerException& e )
{
ROS_ERROR("[robot_observer] Exception was caught : %s",e.description().c_str());
}
}
