/*!
* \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 Constructor for PowercubeChainNode class.
*
* \param name Name for the actionlib server.
*/
PowercubeChainNode(std::string name):
as_(n_, name, boost::bind(&PowercubeChainNode::executeCB, this, _1)),
action_name_(name)
{
sem_can_available = false;
can_sem = SEM_FAILED;
isInitialized_ = false;
traj_point_nr_ = 0;
finished_ = false;
#ifndef SIMU
PCube_ = new PowerCubeCtrl();
#else
PCube_ = new simulatedArm();
#endif
PCubeParams_ = new PowerCubeCtrlParams();
// implementation of topics to publish
topicPub_JointState_ = n_.advertise<sensor_msgs::JointState>("/joint_states", 1);
topicPub_ControllerState_ = n_.advertise<pr2_controllers_msgs::JointTrajectoryControllerState>("state", 1);
// implementation of topics to subscribe
topicSub_DirectCommand_ = n_.subscribe("command", 1, &PowercubeChainNode::topicCallback_DirectCommand, this);
// implementation of service servers
srvServer_Init_ = n_.advertiseService("init", &PowercubeChainNode::srvCallback_Init, this);
srvServer_Stop_ = n_.advertiseService("stop", &PowercubeChainNode::srvCallback_Stop, this);
srvServer_Recover_ = n_.advertiseService("recover", &PowercubeChainNode::srvCallback_Recover, this);
srvServer_SetOperationMode_ = n_.advertiseService("set_operation_mode", &PowercubeChainNode::srvCallback_SetOperationMode, this);
// implementation of service clients
//--
// diagnostics
updater_.setHardwareID(ros::this_node::getName());
updater_.add("initialization", this, &PowercubeChainNode::diag_init);
// read parameters from parameter server
n_.getParam("CanModule", CanModule_);
n_.getParam("CanDevice", CanDevice_);
n_.getParam("CanBaudrate", CanBaudrate_);
ROS_INFO("CanModule=%s, CanDevice=%d, CanBaudrate=%d",CanModule_.c_str(),CanDevice_,CanBaudrate_);
// get ModIds from parameter server
if (n_.hasParam("ModIds"))
{
n_.getParam("ModIds", ModIds_param_);
}
else
{
ROS_ERROR("Parameter ModIds not set");
}
ModIds_.resize(ModIds_param_.size());
for (int i = 0; i<ModIds_param_.size(); i++ )
{
ModIds_[i] = (int)ModIds_param_[i];
}
std::cout << "ModIds = " << ModIds_param_ << std::endl;
// get JointNames from parameter server
ROS_INFO("getting JointNames from parameter server");
if (n_.hasParam("JointNames"))
{
n_.getParam("JointNames", JointNames_param_);
}
else
{
ROS_ERROR("Parameter JointNames not set");
}
JointNames_.resize(JointNames_param_.size());
for (int i = 0; i<JointNames_param_.size(); i++ )
{
JointNames_[i] = (std::string)JointNames_param_[i];
}
std::cout << "JointNames = " << JointNames_param_ << std::endl;
PCubeParams_->Init(CanModule_, CanDevice_, CanBaudrate_, ModIds_);
// get MaxAcc from parameter server
ROS_INFO("getting MaxAcc from parameter server");
if (n_.hasParam("MaxAcc"))
{
n_.getParam("MaxAcc", MaxAcc_param_);
}
else
{
ROS_ERROR("Parameter MaxAcc not set");
}
MaxAcc_.resize(MaxAcc_param_.size());
for (int i = 0; i<MaxAcc_param_.size(); i++ )
{
MaxAcc_[i] = (double)MaxAcc_param_[i];
}
PCubeParams_->SetMaxAcc(MaxAcc_);
std::cout << "MaxAcc = " << MaxAcc_param_ << std::endl;
// load parameter server string for robot/model
std::string param_name = "robot_description";
std::string full_param_name;
std::string xml_string;
n_.searchParam(param_name,full_param_name);
n_.getParam(full_param_name.c_str(),xml_string);
ROS_INFO("full_param_name=%s",full_param_name.c_str());
if (xml_string.size()==0)
{
ROS_ERROR("Unable to load robot model from param server robot_description\n");
exit(2);
}
ROS_DEBUG("%s content\n%s", full_param_name.c_str(), xml_string.c_str());
// extract limits and velocities from urdf model
urdf::Model model;
if (!model.initString(xml_string))
{
ROS_ERROR("Failed to parse urdf file");
exit(2);
}
ROS_INFO("Successfully parsed urdf file");
/** @todo: check if yaml parameter file fits to urdf model */
// get MaxVel out of urdf model
std::vector<double> MaxVel;
MaxVel.resize(ModIds_param_.size());
for (int i = 0; i<ModIds_param_.size(); i++ )
{
MaxVel[i] = model.getJoint(JointNames_[i].c_str())->limits->velocity;
//std::cout << "MaxVel[" << JointNames_[i].c_str() << "] = " << MaxVel[i] << std::endl;
}
PCubeParams_->SetMaxVel(MaxVel);
// get LowerLimits out of urdf model
std::vector<double> LowerLimits;
LowerLimits.resize(ModIds_param_.size());
for (int i = 0; i<ModIds_param_.size(); i++ )
{
LowerLimits[i] = model.getJoint(JointNames_[i].c_str())->limits->lower;
std::cout << "LowerLimits[" << JointNames_[i].c_str() << "] = " << LowerLimits[i] << std::endl;
}
PCubeParams_->SetLowerLimits(LowerLimits);
// get UpperLimits out of urdf model
std::vector<double> UpperLimits;
UpperLimits.resize(ModIds_param_.size());
for (int i = 0; i<ModIds_param_.size(); i++ )
{
UpperLimits[i] = model.getJoint(JointNames_[i].c_str())->limits->upper;
std::cout << "UpperLimits[" << JointNames_[i].c_str() << "] = " << UpperLimits[i] << std::endl;
}
PCubeParams_->SetUpperLimits(UpperLimits);
// get UpperLimits out of urdf model
std::vector<double> Offsets;
Offsets.resize(ModIds_param_.size());
for (int i = 0; i<ModIds_param_.size(); i++ )
{
Offsets[i] = model.getJoint(JointNames_[i].c_str())->calibration->rising.get()[0];
std::cout << "Offset[" << JointNames_[i].c_str() << "] = " << Offsets[i] << std::endl;
}
PCubeParams_->SetAngleOffsets(Offsets);
cmd_vel_.resize(ModIds_param_.size());
newvel_ = false;
can_sem = sem_open("/semcan", O_CREAT, S_IRWXU | S_IRWXG,1);
if (can_sem != SEM_FAILED)
sem_can_available = true;
}
/*!
* \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);
}
/*!
* \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);
}
/*!
* \brief Gets parameters from the robot_description and configures the powercube_chain.
*/
void getRobotDescriptionParameters()
{
unsigned int DOF = pc_params_->GetDOF();
std::vector<std::string> JointNames = pc_params_->GetJointNames();
/// Get robot_description from ROS parameter server
std::string param_name = "robot_description";
std::string full_param_name;
std::string xml_string;
n_.searchParam(param_name, full_param_name);
if (n_.hasParam(full_param_name))
{
n_.getParam(full_param_name.c_str(), xml_string);
}
else
{
ROS_ERROR("Parameter %s not set, shutting down node...", full_param_name.c_str());
n_.shutdown();
}
if (xml_string.size() == 0)
{
ROS_ERROR("Unable to load robot model from parameter %s",full_param_name.c_str());
n_.shutdown();
}
ROS_DEBUG("%s content\n%s", full_param_name.c_str(), xml_string.c_str());
/// Get urdf model out of robot_description
urdf::Model model;
if (!model.initString(xml_string))
{
ROS_ERROR("Failed to parse urdf file");
n_.shutdown();
}
ROS_DEBUG("Successfully parsed urdf file");
/// Get max velocities out of urdf model
std::vector<double> MaxVelocities(DOF);
for (unsigned int i = 0; i < DOF; i++)
{
MaxVelocities[i] = model.getJoint(JointNames[i].c_str())->limits->velocity;
}
/// Get lower limits out of urdf model
std::vector<double> LowerLimits(DOF);
for (unsigned int i = 0; i < DOF; i++)
{
LowerLimits[i] = model.getJoint(JointNames[i].c_str())->limits->lower;
}
// Get upper limits out of urdf model
std::vector<double> UpperLimits(DOF);
for (unsigned int i = 0; i < DOF; i++)
{
UpperLimits[i] = model.getJoint(JointNames[i].c_str())->limits->upper;
}
/// Get offsets out of urdf model
std::vector<double> Offsets(DOF);
for (unsigned int i = 0; i < DOF; i++)
{
Offsets[i] = model.getJoint(JointNames[i].c_str())->calibration->rising.get()[0];
}
/// Set parameters
pc_params_->SetMaxVel(MaxVelocities);
pc_params_->SetLowerLimits(LowerLimits);
pc_params_->SetUpperLimits(UpperLimits);
pc_params_->SetOffsets(Offsets);
}
/*!
* \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);
}