/*!
* \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");
}
}
// 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());
}
}