C++ (Cpp) States::GetDistance Examples

Example #1

File: experiment.cpp Project: smilingpaul/marhes-ros-pkg

void Experiment::timer_cb(const ros::TimerEvent& event)
{
	switch(mode_)
	{
	case MODE_REP_START:
		state_ = (int)states_->GetState();
		ROS_INFO("Starting rep: %d", cnt_rep_);
		mode_ = MODE_REP;
		break;
	case MODE_REP:
		action_ = qobj_->GetAction(state_);
		actions_->Move((Actions::moveType)action_);
		state_p_ = (int)states_->GetState();

		ROS_INFO("Action: %d, produced state: %d", action_, state_p_);

		if (learn_)
		{
			reward_ = states_->GetReward();
			qobj_->Update(reward_, state_, state_p_, action_);
			ROS_INFO("Action: %d, produced state: %d with reward: %f", action_, state_p_, reward_);
			ROS_INFO("Table: \n%s", qobj_->PrintTable().c_str());
		}

		state_ = state_p_;

		if (states_->GetDistance() < radius_)
		{
			mode_ = MODE_RETURN;
			ROS_INFO("Completed rep: %d, returning to start location", cnt_rep_);

			// Move to starting spot
			srv_.request.pose.position.x = start_x_;
			srv_.request.pose.position.y = start_y_;
			client_.call(srv_);
		}
		break;
	case MODE_RETURN:
		if (move_stopped_ == true)
		{
			move_stopped_ = false;
			mode_ = MODE_REP_START;
			cnt_rep_++;
		}

		if (cnt_rep_ > num_reps_)
			mode_ = MODE_DONE;
		break;
	case MODE_DONE:
		break;
	}
}