/*!
* \brief Routine for updating command to the powercubes.
*
* Depending on the operation mode (position/velocity) either position or velocity goals are sent to the hardware.
*/
void updatePCubeCommands()
{
if (isInitialized_ == true)
{
std::string operationMode;
n_.getParam("OperationMode", operationMode);
if (operationMode == "position")
{
ROS_DEBUG("moving powercubes in position mode");
if (PCube_->statusMoving() == false)
{
//feedback_.isMoving = false;
ROS_DEBUG("next point is %d from %d",traj_point_nr_,traj_.points.size());
if (traj_point_nr_ < traj_.points.size())
{
// if powercubes are not moving and not reached last point of trajectory, then send new target point
ROS_DEBUG("...moving to trajectory point[%d]",traj_point_nr_);
traj_point_ = traj_.points[traj_point_nr_];
PCube_->MoveJointSpaceSync(traj_point_.positions);
traj_point_nr_++;
//feedback_.isMoving = true;
//feedback_.pointNr = traj_point_nr;
//as_.publishFeedback(feedback_);
}
else
{
ROS_DEBUG("...reached end of trajectory");
finished_ = true;
}
}
else
{
ROS_DEBUG("...powercubes still moving to point[%d]",traj_point_nr_);
}
}
else if (operationMode == "velocity")
{
ROS_DEBUG("moving powercubes in velocity mode");
if(newvel_)
{
ROS_INFO("MoveVel Call");
PCube_->MoveVel(cmd_vel_);
newvel_ = false;
}
}
else
{
ROS_ERROR("powercubes neither in position nor in velocity mode. OperationMode = [%s]", operationMode.c_str());
}
}
else
{
ROS_DEBUG("powercubes not initialized");
}
}
/*!
* \brief Executes the callback from the actionlib.
*
* Set the current goal to aborted after receiving a new goal and write new goal to a member variable. Wait for the goal to finish and set actionlib status to succeeded.
* \param goal JointTrajectoryGoal
*/
void executeCB(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal)
{
ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
if (!isInitialized_)
{
ROS_ERROR("%s: Rejected, powercubes not initialized", action_name_.c_str());
as_.setAborted();
return;
}
// saving goal into local variables
traj_ = goal->trajectory;
traj_point_nr_ = 0;
traj_point_ = traj_.points[traj_point_nr_];
finished_ = false;
// stoping arm to prepare for new trajectory
std::vector<double> VelZero;
VelZero.resize(ModIds_param_.size());
PCube_->MoveVel(VelZero);
// 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(500000); // needed sleep until powercubes starts to change status from idle to moving
while(finished_ == false)
{
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_);
}
// topic callback functions
// function will be called when a new message arrives on a topic
void topicCallback_JointCommand(const cob3_msgs::JointCommand::ConstPtr& msg)
{
std::string operationMode;
n.getParam("OperationMode", operationMode);
if (operationMode == "position")
{
ROS_INFO("moving powercubes in position mode");
//TODO PowerCubeCtrl
PCube->MoveJointSpaceSync(msg->positions);
}
else if (operationMode == "velocity")
{
ROS_INFO("moving powercubes in velocity mode");
//TODO PowerCubeCtrl
PCube->MoveVel(msg->velocities);
}
else
{
ROS_ERROR("powercubes neither in position nor in velocity mode. OperationMode = [%s]", operationMode.c_str());
}
}
/*!
* \brief Executes the callback from the command_vel topic.
*
* Set the current velocity target.
* \param msg JointVelocities
*/
void topicCallback_CommandVel(const brics_actuator::JointVelocities::ConstPtr& msg)
{
ROS_DEBUG("Received new velocity command");
if (!initialized_)
{
ROS_WARN("Skipping command: powercubes not initialized");
publishState(false);
return;
}
if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK)
{
publishState(false);
return;
}
PowerCubeCtrl::PC_CTRL_STATUS status;
std::vector<std::string> errorMessages;
pc_ctrl_->getStatus(status, errorMessages);
/// ToDo: don't rely on position of joint names, but merge them (check between msg.joint_uri and member variable JointStates)
unsigned int DOF = pc_params_->GetDOF();
std::vector<std::string> jointNames = pc_params_->GetJointNames();
std::vector<double> cmd_vel(DOF);
std::string unit = "rad";
/// check dimensions
if (msg->velocities.size() != DOF)
{
ROS_ERROR("Skipping command: Commanded velocities and DOF are not same dimension.");
return;
}
/// parse velocities
for (unsigned int i = 0; i < DOF; i++)
{
/// check joint name
if (msg->velocities[i].joint_uri != jointNames[i])
{
ROS_ERROR("Skipping command: Received joint name %s doesn't match expected joint name %s for joint %d.",msg->velocities[i].joint_uri.c_str(),jointNames[i].c_str(),i);
return;
}
/// check unit
if (msg->velocities[i].unit != unit)
{
ROS_ERROR("Skipping command: Received unit %s doesn't match expected unit %s.",msg->velocities[i].unit.c_str(),unit.c_str());
return;
}
/// if all checks are successful, parse the velocity value for this joint
ROS_DEBUG("Parsing velocity %f for joint %s",msg->velocities[i].value,jointNames[i].c_str());
cmd_vel[i] = msg->velocities[i].value;
}
/// command velocities to powercubes
if (!pc_ctrl_->MoveVel(cmd_vel))
{
error_ = true;
error_msg_ = pc_ctrl_->getErrorMessage();
ROS_ERROR("Skipping command: %s",pc_ctrl_->getErrorMessage().c_str());
return;
}
ROS_DEBUG("Executed velocity command");
publishState(false);
}
