/*!
* \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();
}
bool srvCallback_setAcc(cob_trajectory_controller::SetFloat::Request &req, cob_trajectory_controller::SetFloat::Response &res)
{
ROS_INFO("Setting acceleration to %f", req.value.data);
traj_generator_->SetPTPacc(req.value.data);
res.success.data = true;
return true;
}
/*!
* \brief Run the controller.
*
* The Controller generates desired velocities for every joints from the current positions.
*
*/
void run()
{
if(executing_)
{
watchdog_counter = 0;
if (as_.isPreemptRequested() || !ros::ok() || current_operation_mode_ != "velocity")
{
/// set the action state to preempted
executing_ = false;
traj_generator_->isMoving = false;
ROS_INFO("Preempted trajectory action");
return;
}
std::vector<double> des_vel;
if(traj_generator_->step(q_current, des_vel))
{
if(!traj_generator_->isMoving) //Trajectory is finished
{
executing_ = false;
}
brics_actuator::JointVelocities target_joint_vel;
target_joint_vel.velocities.resize(7);
for(unsigned int i=0; i<7; i++)
{
std::stringstream joint_name;
joint_name << "arm_" << (i+1) << "_joint";
target_joint_vel.velocities[i].joint_uri = joint_name.str();
target_joint_vel.velocities[i].unit = "rad";
target_joint_vel.velocities[i].value = des_vel.at(i);
}
/// send everything
joint_vel_pub_.publish(target_joint_vel);
}
else
{
ROS_INFO("An controller error occured!");
executing_ = false;
}
}
else
{
/// WATCHDOG TODO: don't always send
if(watchdog_counter < 10)
{
sensor_msgs::JointState target_joint_position;
target_joint_position.position.resize(7);
brics_actuator::JointVelocities target_joint_vel;
target_joint_vel.velocities.resize(7);
for (unsigned int i = 0; i < 7; i += 1)
{
std::stringstream joint_name;
joint_name << "arm_" << (i+1) << "_joint";
target_joint_vel.velocities[i].joint_uri = joint_name.str();
target_joint_position.position[i] = 0;
target_joint_vel.velocities[i].unit = "rad";
target_joint_vel.velocities[i].value = 0;
}
joint_vel_pub_.publish(target_joint_vel);
joint_pos_pub_.publish(target_joint_position);
}
watchdog_counter++;
}
}
void run()
{
if(executing_)
{
failure_ = false;
watchdog_counter = 0;
//if (as_follow_.isPreemptRequested() || !ros::ok() || current_operation_mode_ != "velocity")
if (!ros::ok() || current_operation_mode_ != "velocity")
{
// set the action state to preempted
executing_ = false;
traj_generator_->isMoving = false;
//as_.setPreempted();
failure_ = true;
return;
}
if (as_follow_.isPreemptRequested())
{
//as_follow_.setAborted()
failure_ = true;
preemted_ = true;
ROS_INFO("Preempted trajectory action");
return;
}
std::vector<double> des_vel;
if(traj_generator_->step(q_current, des_vel))
{
if(!traj_generator_->isMoving) //Finished trajectory
{
executing_ = false;
preemted_ = false;
}
brics_actuator::JointVelocities target_joint_vel;
target_joint_vel.velocities.resize(DOF);
for(int i=0; i<DOF; i++)
{
target_joint_vel.velocities[i].joint_uri = JointNames_[i].c_str();
target_joint_vel.velocities[i].unit = "rad";
target_joint_vel.velocities[i].value = des_vel.at(i);
}
//send everything
joint_vel_pub_.publish(target_joint_vel);
}
else
{
ROS_INFO("An controller error occured!");
failure_ = true;
executing_ = false;
}
}
else
{ //WATCHDOG TODO: don't always send
if(watchdog_counter < 10)
{
brics_actuator::JointVelocities target_joint_vel;
target_joint_vel.velocities.resize(DOF);
for (int i = 0; i < DOF; i += 1)
{
target_joint_vel.velocities[i].joint_uri = JointNames_[i].c_str();
target_joint_vel.velocities[i].unit = "rad";
target_joint_vel.velocities[i].value = 0;
}
joint_vel_pub_.publish(target_joint_vel);
ROS_INFO("Publishing 0-vel (%d)", DOF);
}
watchdog_counter++;
}
}
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;
}
}
}
