From cee517fd2c0962d3232071e02ebff47f99227cc0 Mon Sep 17 00:00:00 2001 From: Emanuele Date: Wed, 23 Oct 2019 17:37:46 +0200 Subject: [PATCH] Homing funzionante, da perfezionare. Tutte le posizioni di default fanno parte del file .yaml --- .../include/roboglue_ros/roboglue_utils.h | 3 +- src/roboglue_ros/src/roboglue_follower.cpp | 42 +++++++------------ src/roboglue_ros/src/roboglue_utils.cpp | 8 ++++ 3 files changed, 25 insertions(+), 28 deletions(-) diff --git a/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h b/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h index 08d5193..9f2e4f4 100644 --- a/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h +++ b/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h @@ -97,7 +97,7 @@ typedef struct { robot_model::RobotModelConstPtr kine_model; robot_state::RobotStatePtr kine_state; std::vector joint_names; - std::map joint_homes; + std::map joint_homes; } robotData; typedef struct { @@ -110,6 +110,7 @@ trajectory_msgs::JointTrajectory jstr2Joint(nlohmann::json); void getLockInfo(positionData*, nlohmann::json); geometry_msgs::TwistStamped lockTwistPose(geometry_msgs::TwistStamped, geometry_msgs::TwistStamped, positionData); geometry_msgs::PoseStamped twist2Pose(geometry_msgs::TwistStamped); +std::map floatMap2doubleMap(std::map); double e3dist(geometry_msgs::Twist, geometry_msgs::Twist); double rad2deg(double ang); double deg2rad(double ang); diff --git a/src/roboglue_ros/src/roboglue_follower.cpp b/src/roboglue_ros/src/roboglue_follower.cpp index 9d67006..d81c253 100644 --- a/src/roboglue_ros/src/roboglue_follower.cpp +++ b/src/roboglue_ros/src/roboglue_follower.cpp @@ -21,8 +21,7 @@ ///////// FUNCTION DECLARATIONS /////////// ////////////////////////////////////////// void testFunction( ros::Publisher* trj_pub); -trajectory_msgs::JointTrajectory composeTrjMessage(std::vector jtpos); -void resetModelJointsHome(robotData*); +trajectory_msgs::JointTrajectory composeTrjMessage(std::vector jtpos, std::vector jnames); /////////////////////////////////////////// ///////// ROS CALLBACKS ////////////////// @@ -47,20 +46,19 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p } else ROS_ERROR("Invalid acton"); } 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 - resetModelJointsHome(rdata); - rdata->kine_state->setVariablePositions(rdata->joint_homes); + rdata->kine_state->setVariablePositions(floatMap2doubleMap(rdata->joint_homes)); if (!params["execute"].get()){ ROS_DEBUG("Autosetting joints to Home Position"); - ROS_WARN("Remote execution of homing not SAFE yet!!"); + 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])); } - pubs->at("trj_pub")->publish(composeTrjMessage(jPos)); + pubs->at("trj_pub")->publish(composeTrjMessage(jPos, rdata->joint_names)); } else { ROS_WARN("Joint model state set to Home, robot still in last position!!"); } @@ -73,7 +71,7 @@ void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, p } void coordinateCallback(const std_msgs::String::ConstPtr& msg, publisherMap* pubs, positionData* pos){ - ROS_DEBUG("Received coordinate: [%s]", msg->data.c_str()); + ROS_DEBUG_COND(false, "Received coordinate: [%s]", msg->data.c_str()); nlohmann::json coordinateParsed = nlohmann::json::parse(msg->data); std::string type = coordinateParsed["type"]; std::string mode = coordinateParsed["mode"]; @@ -126,7 +124,7 @@ void timedPublishCallback(const ros::TimerEvent&, internalState* is, publisherMa double timeout = 0.1; uint attempts = 10; bool foundIK = false; - std::vector jPos, jStartPos = {M_PI/2, -3*M_PI/4, +3*M_PI/4, -M_PI/2, -M_PI/2, 0}; + std::vector jPos; if (is->isRealtime){ twPose = pos->twist; @@ -141,17 +139,17 @@ void timedPublishCallback(const ros::TimerEvent&, internalState* is, publisherMa newPose.pose.position.x, newPose.pose.position.y, newPose.pose.position.z, newPose.pose.orientation.x, newPose.pose.orientation.y, newPose.pose.orientation.z, newPose.pose.orientation.w); if (firstIK) { - rdata->kine_state->setVariablePositions(jStartPos); + rdata->kine_state->setVariablePositions(floatMap2doubleMap(rdata->joint_homes)); for (auto jj : rdata->kine_state->getVariableNames()) ROS_DEBUG("jointname: %s", jj.c_str()); - rdata->kine_state->copyJointGroupPositions(rdata->joint_modelGroup, jStartPos); + rdata->kine_state->copyJointGroupPositions(rdata->joint_modelGroup, jPos); rdata->kine_state->updateLinkTransforms(); ROS_DEBUG_STREAM("Cart Start Position: " << rdata->kine_state->getGlobalLinkTransform("ee_link").translation() << std::endl); ROS_DEBUG_COND(true, "Joint Start Position: J1:%6.3f, J2:%6.3f, J3:%6.3f \n\ J4:%6.3f, J5:%6.3f, J6:%6.3f", - jStartPos[0], jStartPos[1],jStartPos[2], - jStartPos[3], jStartPos[4],jStartPos[5]); - rdata->kine_state->setJointGroupPositions(rdata->joint_modelGroup, jStartPos); + jPos[0], jPos[1],jPos[2], + jPos[3], jPos[4],jPos[5]); + rdata->kine_state->setVariablePositions(floatMap2doubleMap(rdata->joint_homes)); firstIK = false; } foundIK = rdata->kine_state->setFromIK(rdata->joint_modelGroup, newPose.pose, attempts, timeout); @@ -160,7 +158,7 @@ void timedPublishCallback(const ros::TimerEvent&, internalState* is, publisherMa for (std::size_t i = 0; i < rdata->joint_names.size(); ++i) { ROS_DEBUG_COND(false,"Joint %s: %f", rdata->joint_names[i].c_str(), rad2deg(jPos[i])); } - pubs->at("trj_pub")->publish(composeTrjMessage(jPos)); + pubs->at("trj_pub")->publish(composeTrjMessage(jPos, rdata->joint_names)); } else { ROS_WARN("No IK solution found"); } @@ -262,23 +260,13 @@ int main(int argc, char **argv) { //////////////////////////////////////// ///////////// END MAIN ///////////////// //////////////////////////////////////// - -void resetModelJointsHome(robotData* rdata) { - ROS_DEBUG("Setting joint position to home"); - for(auto joint : rdata->joint_homes){ - joint.second = deg2rad(joint.second); - rdata->joint_homes[joint.first] = joint.second; - } -} - -//TODO: rendere dinamici i nomi dei giunti caricati in robot_data -trajectory_msgs::JointTrajectory composeTrjMessage(std::vector jtpos){ +trajectory_msgs::JointTrajectory composeTrjMessage(std::vector jtpos, std::vector jnames){ static trajectory_msgs::JointTrajectory trj; static double ang=0.0; trj.header.seq++; trj.header.stamp = ros::Time::now(); trj.points.clear(); - trj.joint_names = {"shoulder_pan_joint","shoulder_lift_joint","elbow_joint","wrist_1_joint","wrist_2_joint","wrist_3_joint"}; + trj.joint_names = jnames; trajectory_msgs::JointTrajectoryPoint pt; pt.positions = jtpos; pt.time_from_start.fromSec(0.5); diff --git a/src/roboglue_ros/src/roboglue_utils.cpp b/src/roboglue_ros/src/roboglue_utils.cpp index 18b86f1..153a05f 100644 --- a/src/roboglue_ros/src/roboglue_utils.cpp +++ b/src/roboglue_ros/src/roboglue_utils.cpp @@ -70,6 +70,14 @@ geometry_msgs::PoseStamped twist2Pose(geometry_msgs::TwistStamped twt){ return newPoseStamp; } +std::map floatMap2doubleMap(std::map m) { + std::map tmp; + for (auto j : m){ + tmp[j.first] = (deg2rad(static_cast(j.second))); + } + return tmp; +} + double e3dist(geometry_msgs::Twist p1, geometry_msgs::Twist p2){ double res; res = pow(p1.linear.x - p2.linear.x, 2.0) +