/*!
* \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++;
}
}
