/*!
* \brief Executes a valid trajectory.
*
* Move the arm to a goal configuration in Joint Space
* \param goal JointTrajectoryGoalConstPtr
*/
void executeTrajectory(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal)
{
ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
if(!executing_)
{
while(current_operation_mode_ != "velocity")
{
ROS_INFO("waiting for arm to go to velocity mode");
usleep(100000);
}
traj_ = goal->trajectory;
if(traj_.points.size() == 1)
{
traj_generator_->moveThetas(traj_.points[0].positions, q_current);
}
else
{
//Insert the current point as first point of trajectory, then generate SPLINE trajectory
trajectory_msgs::JointTrajectoryPoint p;
p.positions.resize(7);
p.velocities.resize(7);
p.accelerations.resize(7);
for(unsigned int i = 0; i<7; i++)
{
p.positions.at(i) = q_current.at(i);
p.velocities.at(i) = 0.0;
p.accelerations.at(i) = 0.0;
}
std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator it;
it = traj_.points.begin();
traj_.points.insert(it,p);
traj_generator_->moveTrajectory(traj_, q_current);
}
executing_ = true;
startposition_ = q_current;
//TODO: std::cout << "Trajectory time: " << traj_end_time_ << " Trajectory step: " << traj_step_ << "\n";
}
else //suspend current movement and start new one
{
}
while(executing_)
{
sleep(1);
}
as_.setSucceeded();
}
void spawnTrajector(trajectory_msgs::JointTrajectory trajectory)
{
if(!executing_ || preemted_)
{
//set component to velocity mode
cob_srvs::SetOperationMode opmode;
opmode.request.operation_mode.data = "velocity";
srvClient_SetOperationMode.call(opmode);
ros::Time begin = ros::Time::now();
while(current_operation_mode_ != "velocity")
{
ROS_INFO("waiting for component to go to velocity mode");
usleep(100000);
//add timeout and set action to rejected
if((ros::Time::now() - begin).toSec() > velocity_timeout_)
{
rejected_ = true;
return;
}
}
std::vector<double> traj_start;
if(preemted_ == true) //Calculate trajectory for runtime modification of trajectories
{
ROS_INFO("There is a old trajectory currently running");
traj_start = traj_generator_->last_q;
trajectory_msgs::JointTrajectory temp_traj;
temp_traj = trajectory;
//Insert the saved point as first point of trajectory, then generate SPLINE trajectory
trajectory_msgs::JointTrajectoryPoint p;
p.positions.resize(DOF);
p.velocities.resize(DOF);
p.accelerations.resize(DOF);
for(int i = 0; i<DOF; i++)
{
p.positions.at(i) = traj_start.at(i);
p.velocities.at(i) = 0.0;
p.accelerations.at(i) = 0.0;
}
std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator it;
it = temp_traj.points.begin();
temp_traj.points.insert(it,p);
//Now insert the current as first point of trajectory, then generate SPLINE trajectory
for(int i = 0; i<DOF; i++)
{
p.positions.at(i) = traj_generator_->last_q1.at(i);
p.velocities.at(i) = 0.0;
p.accelerations.at(i) = 0.0;
}
it = temp_traj.points.begin();
temp_traj.points.insert(it,p);
for(int i = 0; i<DOF; i++)
{
p.positions.at(i) = traj_generator_->last_q2.at(i);
p.velocities.at(i) = 0.0;
p.accelerations.at(i) = 0.0;
}
it = temp_traj.points.begin();
temp_traj.points.insert(it,p);
for(int i = 0; i<DOF; i++)
{
p.positions.at(i) = traj_generator_->last_q3.at(i);
p.velocities.at(i) = 0.0;
p.accelerations.at(i) = 0.0;
}
it = temp_traj.points.begin();
temp_traj.points.insert(it,p);
for(int i = 0; i<DOF; i++)
{
p.positions.at(i) = q_current.at(i);
p.velocities.at(i) = 0.0;
p.accelerations.at(i) = 0.0;
}
it = temp_traj.points.begin();
temp_traj.points.insert(it,p);
traj_generator_->isMoving = false ;
traj_generator_->moveTrajectory(temp_traj, traj_start);
}
else //Normal calculation of trajectories
{
traj_start = q_current;
trajectory_msgs::JointTrajectory temp_traj;
temp_traj = trajectory;
if(temp_traj.points.size() == 1)
{
traj_generator_->isMoving = false ;
traj_generator_->moveThetas(temp_traj.points[0].positions, traj_start);
}
else
{
//Insert the current point as first point of trajectory, then generate SPLINE trajectory
trajectory_msgs::JointTrajectoryPoint p;
p.positions.resize(DOF);
p.velocities.resize(DOF);
p.accelerations.resize(DOF);
for(int i = 0; i<DOF; i++)
{
p.positions.at(i) = traj_start.at(i);
p.velocities.at(i) = 0.0;
p.accelerations.at(i) = 0.0;
}
std::vector<trajectory_msgs::JointTrajectoryPoint>::iterator it;
it = temp_traj.points.begin();
temp_traj.points.insert(it,p);
traj_generator_->isMoving = false ;
traj_generator_->moveTrajectory(temp_traj, traj_start);
}
}
executing_ = true;
startposition_ = q_current;
preemted_ = false;
}
else //suspend current movement and start new one
{
}
while(executing_)
{
if(!preemted_)
{
usleep(1000);
}
else
{
return;
}
}
}
