Obbart/RoboGlue_ROS
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

piccole modifiche

Browse Source
This commit is contained in:
Emanuele
2019-10-23 17:49:03 +02:00
parent cee517fd2c
commit aad972404d
1 changed files with 7 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
src/roboglue_ros/src/roboglue_follower.cpp
View File

@@ -48,16 +48,15 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p
} else if (!command.compare("RECORD") || !command.compare("PLAY") || !command.compare("OPEN")) {
return;
} else if (!command.compare("HOME")) {
// Chiamata alla funzione di reset dei giunti nel modello
std::vector<double> jPos;
ROS_DEBUG("Autosetting joints to Home Position");
rdata->kine_state->setVariablePositions(floatMap2doubleMap(rdata->joint_homes));
rdata->kine_state->copyJointGroupPositions(rdata->joint_modelGroup, jPos); // verifica se ha avuto successo rileggendo le posizioni dal modello
for (std::size_t i = 0; i < rdata->joint_names.size(); ++i) {
ROS_DEBUG_COND(true,"Joint %s: %f", rdata->joint_names[i].c_str(), rad2deg(jPos[i]));
}
if (!params["execute"].get<bool>()){
ROS_DEBUG("Autosetting joints to Home Position");
ROS_WARN("Remote execution of homing not SAFE YET!!");
std::vector<double> jPos;
rdata->kine_state->copyJointGroupPositions(rdata->joint_modelGroup, jPos);
for (std::size_t i = 0; i < rdata->joint_names.size(); ++i) {
ROS_DEBUG_COND(true,"Joint %s: %f", rdata->joint_names[i].c_str(), rad2deg(jPos[i]));
}
ROS_WARN("Remote execution of homing NOT SAFE YET!!");
pubs->at("trj_pub")->publish(composeTrjMessage(jPos, rdata->joint_names));
} else {
ROS_WARN("Joint model state set to Home, robot still in last position!!");