bool srvCallback_Stop(cob_srvs::Trigger::Request &req,
cob_srvs::Trigger::Response &res )
{
ROS_INFO("Stopping camera axis");
// stopping all arm movements
if (CamAxis_->Stop()) {
ROS_INFO("Stopping camera axis succesful");
res.success = 0; // 0 = true, else = false
}
else {
ROS_ERROR("Stopping camera axis not succesful. error");
}
return true;
}
//void executeCB(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal) {
void executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) {
if(isInitialized_) {
ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
// saving goal into local variables
traj_ = goal->trajectory;
traj_point_nr_ = 0;
traj_point_ = traj_.points[traj_point_nr_];
GoalPos_ = traj_point_.positions[0];
finished_ = false;
// stoping axis to prepare for new trajectory
CamAxis_->Stop();
// check that preempt has not been requested by the client
if (as_.isPreemptRequested())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
}
usleep(2000000); // needed sleep until drive starts to change status from idle to moving
while (not finished_)
{
if (as_.isNewGoalAvailable())
{
ROS_WARN("%s: Aborted", action_name_.c_str());
as_.setAborted();
return;
}
usleep(10000);
//feedback_ =
//as_.send feedback_
}
// set the action state to succeed
//result_.result.data = "executing trajectory";
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
} else {
as_.setAborted();
ROS_WARN("Camera_axis not initialized yet!");
}
}
bool srvCallback_Stop(cob_srvs::Trigger::Request &req,
cob_srvs::Trigger::Response &res )
{
ROS_INFO("Stopping camera axis");
// stopping all arm movements
if (CamAxis_->Stop()) {
ROS_INFO("Stopping camera axis successful");
res.success.data = true;
res.error_message.data = "camera axis stopped successfully";
}
else {
ROS_ERROR("Stopping camera axis not succesful. error");
res.success.data = false;
res.error_message.data = "stopping camera axis not successful";
}
return true;
}