/*!
* \brief Executes the service callback for recover.
*
* Recovers the driver after an emergency stop.
* \param req Service request
* \param res Service response
*/
bool srvCallback_Recover( cob_srvs::Trigger::Request &req,
cob_srvs::Trigger::Response &res )
{
if (isInitialized_ == true)
{
ROS_INFO("Recovering powercubes");
// stopping all arm movements
if (PCube_->Stop())
{
ROS_INFO("Recovering powercubes succesfull");
res.success.data = true;
}
else
{
ROS_ERROR("Recovering powercubes not succesfull. error: %s", PCube_->getErrorMessage().c_str());
res.success.data = false;
res.error_message.data = PCube_->getErrorMessage();
}
}
else
{
ROS_ERROR("...powercubes already recovered...");
res.success.data = false;
res.error_message.data = "powercubes already recovered";
}
return true;
}
/*!
* \brief Executes the service callback for init.
*
* Connects to the hardware and initialized it.
* \param req Service request
* \param res Service response
*/
bool srvCallback_Init( cob_srvs::Trigger::Request &req,
cob_srvs::Trigger::Response &res )
{
if (isInitialized_ == false)
{
ROS_INFO("...initializing powercubes...");
// init powercubes
if (PCube_->Init(PCubeParams_))
{
ROS_INFO("Initializing succesfull");
isInitialized_ = true;
res.success.data = true;
res.error_message.data = "success";
}
else
{
ROS_ERROR("Initializing powercubes not succesfull. error: %s", PCube_->getErrorMessage().c_str());
res.success.data = false;
res.error_message.data = PCube_->getErrorMessage();
}
}
else
{
ROS_ERROR("...powercubes already initialized...");
res.success.data = false;
res.error_message.data = "powercubes already initialized";
}
return true;
}
/*!
* \brief Executes the service callback for recover.
*
* Recovers the driver after an emergency stop.
* \param req Service request
* \param res Service response
*/
bool srvCallback_Recover(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res)
{
ROS_INFO("Recovering powercubes...");
if (initialized_)
{
/// stopping all arm movements
if (pc_ctrl_->Recover())
{
error_ = false;
error_msg_ = "";
res.success.data = true;
ROS_INFO("...recovering powercubes successful.");
}
else
{
res.success.data = false;
error_ = true;
error_msg_ = pc_ctrl_->getErrorMessage();
res.error_message.data = pc_ctrl_->getErrorMessage();
ROS_ERROR("...recovering powercubes not successful. error: %s", res.error_message.data.c_str());
}
}
else
{
res.success.data = false;
res.error_message.data = "powercubes not initialized";
ROS_ERROR("...recovering powercubes not successful. error: %s",res.error_message.data.c_str());
}
return true;
}
/*!
* \brief Executes the service callback for init.
*
* Connects to the hardware and initialized it.
* \param req Service request
* \param res Service response
*/
bool srvCallback_Init(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res)
{
if (!initialized_)
{
ROS_INFO("Initializing powercubes...");
/// initialize powercubes
if (pc_ctrl_->Init(pc_params_))
{
initialized_ = true;
res.success.data = true;
ROS_INFO("...initializing powercubes successful");
}
else
{
error_ = true;
error_msg_ = pc_ctrl_->getErrorMessage();
res.success.data = false;
res.error_message.data = pc_ctrl_->getErrorMessage();
ROS_INFO("...initializing powercubes not successful. error: %s", res.error_message.data.c_str());
}
}
else
{
res.success.data = true;
res.error_message.data = "powercubes already initialized";
ROS_WARN("...initializing powercubes not successful. error: %s",res.error_message.data.c_str());
}
return true;
}
/*!
* \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 Routine for publishing joint_states.
*
* Gets current positions and velocities from the hardware and publishes them as joint_states.
*/
void publishJointState()
{
if (isInitialized_ == true)
{
// create joint_state message
int DOF = ModIds_param_.size();
std::vector<double> ActualPos;
std::vector<double> ActualVel;
ActualPos.resize(DOF);
ActualVel.resize(DOF);
PCube_->getConfig(ActualPos);
PCube_->getJointVelocities(ActualVel);
sensor_msgs::JointState msg;
msg.header.stamp = ros::Time::now();
msg.name.resize(DOF);
msg.position.resize(DOF);
msg.velocity.resize(DOF);
msg.name = JointNames_;
for (int i = 0; i<DOF; i++ )
{
msg.position[i] = ActualPos[i];
msg.velocity[i] = ActualVel[i];
//std::cout << "Joint " << msg.name[i] <<": pos="<< msg.position[i] << "vel=" << msg.velocity[i] << std::endl;
}
// publish message
topicPub_JointState_.publish(msg);
pr2_controllers_msgs::JointTrajectoryControllerState controllermsg;
controllermsg.header.stamp = ros::Time::now();
controllermsg.joint_names.resize(DOF);
controllermsg.actual.positions.resize(DOF);
controllermsg.actual.velocities.resize(DOF);
controllermsg.joint_names = JointNames_;
for (int i = 0; i<DOF; i++ )
{
controllermsg.actual.positions[i] = ActualPos[i];
controllermsg.actual.velocities[i] = ActualVel[i];
//std::cout << "Joint " << msg.name[i] <<": pos="<< msg.position[i] << "vel=" << msg.velocity[i] << std::endl;
}
topicPub_ControllerState_.publish(controllermsg);
}
}
/*!
* \brief Executes the service callback for stop.
*
* Stops all hardware movements.
* \param req Service request
* \param res Service response
*/
bool srvCallback_Stop(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res)
{
ROS_INFO("Stopping powercubes...");
// stop powercubes
if (pc_ctrl_->Stop())
{
res.success.data = true;
ROS_INFO("...stopping powercubes successful.");
}
else
{
res.success.data = false;
res.error_message.data = pc_ctrl_->getErrorMessage();
ROS_ERROR("...stopping powercubes not successful. error: %s", res.error_message.data.c_str());
}
return true;
}
bool srvCallback_Stop(cob3_srvs::Stop::Request &req,
cob3_srvs::Stop::Response &res )
{
ROS_INFO("Stopping powercubes");
// stopping all arm movements
if (PCube->Stop())
{
ROS_INFO("Stopping powercubes succesfull");
res.success = 0; // 0 = true, else = false
}
else
{
ROS_ERROR("Stopping powercubes not succesfull. error: %s", PCube->getErrorMessage().c_str());
res.success = 1; // 0 = true, else = false
res.errorMessage.data = PCube->getErrorMessage();
}
return true;
}
// 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 Publishes the state of the powercube_chain as ros messages.
*
* Published to "/joint_states" as "sensor_msgs/JointState"
* Published to "state" as "pr2_controllers_msgs/JointTrajectoryState"
*/
void publishState()
{
if (initialized_)
{
ROS_INFO("publish state");
pc_ctrl_->updateStates();
sensor_msgs::JointState joint_state_msg;
joint_state_msg.header.stamp = ros::Time::now();
joint_state_msg.name = pc_params_->GetJointNames();
joint_state_msg.position = pc_ctrl_->getPositions();
joint_state_msg.velocity = pc_ctrl_->getVelocities();
pr2_controllers_msgs::JointTrajectoryControllerState controller_state_msg;
controller_state_msg.header.stamp = joint_state_msg.header.stamp;
controller_state_msg.joint_names = pc_params_->GetJointNames();
controller_state_msg.actual.positions = pc_ctrl_->getPositions();
controller_state_msg.actual.velocities = pc_ctrl_->getVelocities();
controller_state_msg.actual.accelerations = pc_ctrl_->getAccelerations();
topicPub_JointState_.publish(joint_state_msg);
topicPub_ControllerState_.publish(controller_state_msg);
last_publish_time_ = ros::Time::now();
}
}
/*!
* \brief Executes the service callback for stop.
*
* Stops all hardware movements.
* \param req Service request
* \param res Service response
*/
bool srvCallback_Stop( cob_srvs::Trigger::Request &req,
cob_srvs::Trigger::Response &res )
{
ROS_INFO("Stopping powercubes");
newvel_ = false;
// set current trajectory to be finished
traj_point_nr_ = traj_.points.size();
// stopping all arm movements
if (PCube_->Stop())
{
ROS_INFO("Stopping powercubes succesfull");
res.success.data = true;
}
else
{
ROS_ERROR("Stopping powercubes not succesfull. error: %s", PCube_->getErrorMessage().c_str());
res.success.data = false;
res.error_message.data = PCube_->getErrorMessage();
}
return true;
}
/*!
* \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_);
}
// service callback functions
// function will be called when a service is querried
bool srvCallback_Init(cob3_srvs::Init::Request &req,
cob3_srvs::Init::Response &res )
{
ROS_INFO("Initializing powercubes");
// init powercubes
//TODO: make iniFilepath as an argument
//TODO: read iniFile into ros prarameters
if (PCube->Init("include/iniFile.txt"))
{
ROS_INFO("Initializing succesfull");
res.success = 0; // 0 = true, else = false
}
else
{
ROS_ERROR("Initializing powercubes not succesfull. error: %s", PCube->getErrorMessage().c_str());
res.success = 1; // 0 = true, else = false
res.errorMessage.data = PCube->getErrorMessage();
return true;
}
// homing powercubes
if (PCube->doHoming())
{
ROS_INFO("Homing powercubes succesfull");
res.success = 0; // 0 = true, else = false
}
else
{
ROS_ERROR("Homing powercubes not succesfull. error: %s", PCube->getErrorMessage().c_str());
res.success = 1; // 0 = true, else = false
res.errorMessage.data = PCube->getErrorMessage();
return true;
}
return true;
}
/*!
* \brief Gets parameters from the ROS parameter server and configures the powercube_chain.
*/
void getROSParameters()
{
/// get CanModule
std::string CanModule;
if (n_.hasParam("can_module"))
{
n_.getParam("can_module", CanModule);
}
else
{
ROS_ERROR("Parameter can_module not set, shutting down node...");
n_.shutdown();
}
/// get CanDevice
std::string CanDevice;
if (n_.hasParam("can_device"))
{
n_.getParam("can_device", CanDevice);
}
else
{
ROS_ERROR("Parameter can_device not set, shutting down node...");
n_.shutdown();
}
/// get CanBaudrate
int CanBaudrate;
if (n_.hasParam("can_baudrate"))
{
n_.getParam("can_baudrate", CanBaudrate);
}
else
{
ROS_ERROR("Parameter can_baudrate not set, shutting down node...");
n_.shutdown();
}
/// get Modul IDs
XmlRpc::XmlRpcValue ModulIDsXmlRpc;
std::vector<int> ModulIDs;
if (n_.hasParam("modul_ids"))
{
n_.getParam("modul_ids", ModulIDsXmlRpc);
}
else
{
ROS_ERROR("Parameter modul_ids not set, shutting down node...");
n_.shutdown();
}
/// get force_use_movevel
bool UseMoveVel;
if (n_.hasParam("force_use_movevel"))
{
n_.getParam("force_use_movevel", UseMoveVel);
ROS_INFO("Parameter force_use_movevel set, using moveVel");
}
else
{
ROS_INFO("Parameter force_use_movevel not set, using moveStep");
UseMoveVel = false;
}
pc_params_->SetUseMoveVel(UseMoveVel);
/// Resize and assign of values to the ModulIDs
ModulIDs.resize(ModulIDsXmlRpc.size());
for (int i = 0; i < ModulIDsXmlRpc.size(); i++)
{
ModulIDs[i] = (int)ModulIDsXmlRpc[i];
}
/// Initialize parameters
pc_params_->Init(CanModule, CanDevice, CanBaudrate, ModulIDs);
/// Get joint names
XmlRpc::XmlRpcValue JointNamesXmlRpc;
std::vector<std::string> JointNames;
if (n_.hasParam("joint_names"))
{
n_.getParam("joint_names", JointNamesXmlRpc);
}
else
{
ROS_ERROR("Parameter joint_names not set, shutting down node...");
n_.shutdown();
}
/// Resize and assign of values to the JointNames
JointNames.resize(JointNamesXmlRpc.size());
for (int i = 0; i < JointNamesXmlRpc.size(); i++)
{
JointNames[i] = (std::string)JointNamesXmlRpc[i];
}
/// Check dimension with with DOF
if ((int)JointNames.size() != pc_params_->GetDOF())
{
ROS_ERROR("Wrong dimensions of parameter joint_names, shutting down node...");
n_.shutdown();
}
pc_params_->SetJointNames(JointNames);
/// Get max accelerations
XmlRpc::XmlRpcValue MaxAccelerationsXmlRpc;
std::vector<double> MaxAccelerations;
if (n_.hasParam("max_accelerations"))
{
n_.getParam("max_accelerations", MaxAccelerationsXmlRpc);
}
else
{
ROS_ERROR("Parameter max_accelerations not set, shutting down node...");
n_.shutdown();
}
/// Resize and assign of values to the MaxAccelerations
MaxAccelerations.resize(MaxAccelerationsXmlRpc.size());
for (int i = 0; i < MaxAccelerationsXmlRpc.size(); i++)
{
MaxAccelerations[i] = (double)MaxAccelerationsXmlRpc[i];
}
/// Check dimension with with DOF
if ((int)MaxAccelerations.size() != pc_params_->GetDOF())
{
ROS_ERROR("Wrong dimensions of parameter max_accelerations, shutting down node...");
n_.shutdown();
}
pc_params_->SetMaxAcc(MaxAccelerations);
/// Get horizon
double Horizon;
if (n_.hasParam("horizon"))
{
n_.getParam("horizon", Horizon);
}
else
{
/// Horizon in sec
Horizon = 0.05;
ROS_WARN("Parameter horizon not available, setting to default value: %f sec", Horizon);
}
pc_ctrl_->setHorizon(Horizon);
}
/*!
* \brief Gets parameters from the ROS parameter server and configures the powercube_chain.
*/
void getROSParameters()
{
// get CanModule
std::string CanModule;
if (n_.hasParam("can_module"))
{
n_.getParam("can_module", CanModule);
}
else
{
ROS_ERROR("Parameter can_module not set, shutting down node...");
n_.shutdown();
}
// get CanDevice
std::string CanDevice;
if (n_.hasParam("can_device"))
{
n_.getParam("can_device", CanDevice);
}
else
{
ROS_ERROR("Parameter can_device not set, shutting down node...");
n_.shutdown();
}
// get CanBaudrate
int CanBaudrate;
if (n_.hasParam("can_baudrate"))
{
n_.getParam("can_baudrate", CanBaudrate);
}
else
{
ROS_ERROR("Parameter can_baudrate not set, shutting down node...");
n_.shutdown();
}
// get modul ids
XmlRpc::XmlRpcValue ModulIDsXmlRpc;
std::vector<int> ModulIDs;
if (n_.hasParam("modul_ids"))
{
n_.getParam("modul_ids", ModulIDsXmlRpc);
}
else
{
ROS_ERROR("Parameter modul_ids not set, shutting down node...");
n_.shutdown();
}
ModulIDs.resize(ModulIDsXmlRpc.size());
for (int i = 0; i < ModulIDsXmlRpc.size(); i++)
{
ModulIDs[i] = (int)ModulIDsXmlRpc[i];
}
// init parameters
pc_params_->Init(CanModule, CanDevice, CanBaudrate, ModulIDs);
// get joint names
XmlRpc::XmlRpcValue JointNamesXmlRpc;
std::vector<std::string> JointNames;
if (n_.hasParam("joint_names"))
{
n_.getParam("joint_names", JointNamesXmlRpc);
}
else
{
ROS_ERROR("Parameter joint_names not set, shutting down node...");
n_.shutdown();
}
JointNames.resize(JointNamesXmlRpc.size());
for (int i = 0; i < JointNamesXmlRpc.size(); i++)
{
JointNames[i] = (std::string)JointNamesXmlRpc[i];
}
// check dimension with with DOF
if ((int)JointNames.size() != pc_params_->GetDOF())
{
ROS_ERROR("Wrong dimensions of parameter joint_names, shutting down node...");
n_.shutdown();
}
pc_params_->SetJointNames(JointNames);
// get max accelerations
XmlRpc::XmlRpcValue MaxAccelerationsXmlRpc;
std::vector<double> MaxAccelerations;
if (n_.hasParam("max_accelerations"))
{
n_.getParam("max_accelerations", MaxAccelerationsXmlRpc);
}
else
{
ROS_ERROR("Parameter max_accelerations not set, shutting down node...");
n_.shutdown();
}
MaxAccelerations.resize(MaxAccelerationsXmlRpc.size());
for (int i = 0; i < MaxAccelerationsXmlRpc.size(); i++)
{
MaxAccelerations[i] = (double)MaxAccelerationsXmlRpc[i];
}
// check dimension with with DOF
if ((int)MaxAccelerations.size() != pc_params_->GetDOF())
{
ROS_ERROR("Wrong dimensions of parameter max_accelerations, shutting down node...");
n_.shutdown();
}
pc_params_->SetMaxAcc(MaxAccelerations);
// get horizon
double Horizon;
if (n_.hasParam("horizon"))
{
n_.getParam("horizon", Horizon);
}
else
{
Horizon = 0.025; //Hz
ROS_WARN("Parameter horizon not available, setting to default value: %f sec", Horizon);
}
pc_ctrl_->setHorizon(Horizon);
}
/*!
* \brief Destructor for PowercubeChainNode class.
*/
~PowercubeChainNode()
{
bool closed = PCube_->Close();
if (closed) ROS_INFO("PowerCube Device closed!");
if (sem_can_available) sem_close(can_sem);
}
/*!
* \brief Publishes the state of the powercube_chain as ros messages.
*
* Published to "/joint_states" as "sensor_msgs/JointState"
* Published to "state" as "pr2_controllers_msgs/JointTrajectoryState"
*/
void publishState(bool update=true)
{
if (initialized_)
{
ROS_DEBUG("publish state");
if(update)
{pc_ctrl_->updateStates();}
sensor_msgs::JointState joint_state_msg;
joint_state_msg.header.stamp = ros::Time::now();
joint_state_msg.name = pc_params_->GetJointNames();
joint_state_msg.position = pc_ctrl_->getPositions();
joint_state_msg.velocity = pc_ctrl_->getVelocities();
joint_state_msg.effort.resize(pc_params_->GetDOF());
pr2_controllers_msgs::JointTrajectoryControllerState controller_state_msg;
controller_state_msg.header.stamp = joint_state_msg.header.stamp;
controller_state_msg.joint_names = pc_params_->GetJointNames();
controller_state_msg.actual.positions = pc_ctrl_->getPositions();
controller_state_msg.actual.velocities = pc_ctrl_->getVelocities();
controller_state_msg.actual.accelerations = pc_ctrl_->getAccelerations();
std_msgs::String opmode_msg;
opmode_msg.data = "velocity";
/// publishing joint and controller states on topic
topicPub_JointState_.publish(joint_state_msg);
topicPub_ControllerState_.publish(controller_state_msg);
topicPub_OperationMode_.publish(opmode_msg);
last_publish_time_ = joint_state_msg.header.stamp;
}
// check status of PowerCube chain
if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK) { error_ = true; }
// check status of PowerCube chain
if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK)
{
error_ = true;
}
else
{
error_ = false;
}
// publishing diagnotic messages
diagnostic_msgs::DiagnosticArray diagnostics;
diagnostics.status.resize(1);
// set data to diagnostics
if(error_)
{
diagnostics.status[0].level = 2;
diagnostics.status[0].name = n_.getNamespace();;
diagnostics.status[0].message = pc_ctrl_->getErrorMessage();
}
else
{
if (initialized_)
{
diagnostics.status[0].level = 0;
diagnostics.status[0].name = n_.getNamespace(); //"schunk_powercube_chain";
diagnostics.status[0].message = "powercubechain initialized and running";
}
else
{
diagnostics.status[0].level = 1;
diagnostics.status[0].name = n_.getNamespace(); //"schunk_powercube_chain";
diagnostics.status[0].message = "powercubechain not initialized";
}
}
// publish diagnostic message
topicPub_Diagnostic_.publish(diagnostics);
}
/// Destructor
~PowerCubeChainNode()
{
bool closed = pc_ctrl_->Close();
if (closed)
ROS_INFO("PowerCube Device closed!");
}
/*!
* \brief Routine for publishing joint_states.
*
* Gets current positions and velocities from the hardware and publishes them as joint_states.
*/
void publishJointState()
{
updater_.update();
if (isInitialized_ == true)
{
// publish joint state message
int DOF = ModIds_param_.size();
std::vector<double> ActualPos;
std::vector<double> ActualVel;
ActualPos.resize(DOF);
ActualVel.resize(DOF);
lock_semaphore(can_sem);
int success = PCube_->getConfig(ActualPos);
if (!success) return;
PCube_->getJointVelocities(ActualVel);
unlock_semaphore(can_sem);
sensor_msgs::JointState msg;
msg.header.stamp = ros::Time::now();
msg.name.resize(DOF);
msg.position.resize(DOF);
msg.velocity.resize(DOF);
msg.name = JointNames_;
for (int i = 0; i<DOF; i++ )
{
msg.position[i] = ActualPos[i];
msg.velocity[i] = ActualVel[i];
//std::cout << "Joint " << msg.name[i] <<": pos="<< msg.position[i] << "vel=" << msg.velocity[i] << std::endl;
}
topicPub_JointState_.publish(msg);
// publish controller state message
pr2_controllers_msgs::JointTrajectoryControllerState controllermsg;
controllermsg.header.stamp = ros::Time::now();
controllermsg.joint_names.resize(DOF);
controllermsg.desired.positions.resize(DOF);
controllermsg.desired.velocities.resize(DOF);
controllermsg.actual.positions.resize(DOF);
controllermsg.actual.velocities.resize(DOF);
controllermsg.error.positions.resize(DOF);
controllermsg.error.velocities.resize(DOF);
controllermsg.joint_names = JointNames_;
for (int i = 0; i<DOF; i++ )
{
//std::cout << "Joint " << msg.name[i] <<": pos="<< msg.position[i] << "vel=" << msg.velocity[i] << std::endl;
if (traj_point_.positions.size() != 0)
controllermsg.desired.positions[i] = traj_point_.positions[i];
else
controllermsg.desired.positions[i] = 0.0;
controllermsg.desired.velocities[i] = 0.0;
controllermsg.actual.positions[i] = ActualPos[i];
controllermsg.actual.velocities[i] = ActualVel[i];
controllermsg.error.positions[i] = controllermsg.desired.positions[i] - controllermsg.actual.positions[i];
controllermsg.error.velocities[i] = controllermsg.desired.velocities[i] - controllermsg.actual.velocities[i];
}
topicPub_ControllerState_.publish(controllermsg);
}
}
/*!
* \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);
}
