From aad972404df49cb792c040e381a96816c7ca65f5 Mon Sep 17 00:00:00 2001 From: Emanuele Date: Wed, 23 Oct 2019 17:49:03 +0200 Subject: [PATCH] piccole modifiche --- src/roboglue_ros/src/roboglue_follower.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/roboglue_ros/src/roboglue_follower.cpp b/src/roboglue_ros/src/roboglue_follower.cpp index d81c253..5685bb3 100644 --- a/src/roboglue_ros/src/roboglue_follower.cpp +++ b/src/roboglue_ros/src/roboglue_follower.cpp @@ -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 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()){ - ROS_DEBUG("Autosetting joints to Home Position"); - ROS_WARN("Remote execution of homing not SAFE YET!!"); - std::vector 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!!");