From 6f1f4820a96120adb56d1ca5b98bda409c3f8da6 Mon Sep 17 00:00:00 2001 From: Emanuele Date: Wed, 23 Oct 2019 15:43:10 +0200 Subject: [PATCH] Prima implementazione dell'homing, i giunti vanno tutti a 0 --- .../include/roboglue_ros/roboglue_utils.h | 3 +- src/roboglue_ros/src/roboglue_follower.cpp | 38 +++++++++++++------ src/roboglue_ros/src/roboglue_recorder.cpp | 2 +- src/roboglue_ros/src/roboglue_relay.cpp | 2 +- src/roboglue_ros/src/roboglue_utils.cpp | 4 ++ 5 files changed, 34 insertions(+), 15 deletions(-) diff --git a/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h b/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h index c9d3b32..08d5193 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 { @@ -112,4 +112,5 @@ geometry_msgs::TwistStamped lockTwistPose(geometry_msgs::TwistStamped, geometry_ geometry_msgs::PoseStamped twist2Pose(geometry_msgs::TwistStamped); double e3dist(geometry_msgs::Twist, geometry_msgs::Twist); double rad2deg(double ang); +double deg2rad(double ang); bool checkJointJump(moveit_msgs::RobotTrajectoryPtr, double); diff --git a/src/roboglue_ros/src/roboglue_follower.cpp b/src/roboglue_ros/src/roboglue_follower.cpp index 9faf16f..e5011cc 100644 --- a/src/roboglue_ros/src/roboglue_follower.cpp +++ b/src/roboglue_ros/src/roboglue_follower.cpp @@ -22,12 +22,12 @@ ////////////////////////////////////////// void testFunction( ros::Publisher* trj_pub); trajectory_msgs::JointTrajectory composeTrjMessage(std::vector jtpos); -bool resetModelJointsHome(geometry_msgs::Pose, bool); +void resetModelJointsHome(robotData*); /////////////////////////////////////////// ///////// ROS CALLBACKS ////////////////// ///////////////////////////////////////// -void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, positionData* pos) { +void commandCallback(const std_msgs::String::ConstPtr& msg, internalState* is, publisherMap* pubs, positionData* pos, robotData* rdata) { ROS_DEBUG("Received command: [%s]", msg->data.c_str()); try { nlohmann::json c = nlohmann::json::parse(msg->data.c_str()); @@ -47,9 +47,22 @@ 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 + // Chiamata alla funzione di reset dei giunti nel modello + resetModelJointsHome(rdata); + 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])); + } + pubs->at("trj_pub")->publish(composeTrjMessage(jPos)); + } else { + ROS_WARN("Joint model state set to Home, robot still in last position!!"); + } } else { ROS_WARN("Unknown command: %s", command.c_str()); } @@ -199,7 +212,7 @@ int main(int argc, char **argv) { /// set spinner rate /// ros::Rate loop_rate(loopRate); /// set console log level /// - ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); + ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); ROS_INFO("Follower Node Started"); /// advertise publish topics /// @@ -216,7 +229,7 @@ int main(int argc, char **argv) { publishers["trj_pub"] = &trj_pub; /// subscribe to incoming topics /// ros::Subscriber command_sub = nh.subscribe(ros_topics.commandSub, static_cast(msgBufferSize), - boost::bind(commandCallback, _1, &is, &pos_data)); + boost::bind(commandCallback, _1, &is, &publishers, &pos_data, &robot_data)); ros::Subscriber coordinate_sub = nh.subscribe(ros_topics.coordSub, static_cast(msgBufferSize), boost::bind(coordinateCallback, _1, &publishers, &pos_data)); ros::Subscriber state_sub = nh.subscribe(ros_topics.stateSub, static_cast(msgBufferSize), @@ -249,16 +262,17 @@ int main(int argc, char **argv) { ///////////// END MAIN ///////////////// //////////////////////////////////////// -// TODO: aggiungere la possibilità di resettare i giunti nella configurazione home a comando da interfaccia -bool resetModelJointsHome(geometry_msgs::Pose home_pos, bool execute=false) { +void resetModelJointsHome(robotData* rdata) { ROS_DEBUG("Setting joint position to home"); - if (execute){ - return false; - } else { - return false; + std::map jhome = rdata->joint_homes; + for (auto joint : jhome) { + std::string jname = joint.first; + const double jval = deg2rad(joint.second); + rdata->kine_state->setJointPositions(jname, &jval); } } +//TODO: rendere dinamici i nomi dei giunti caricati in robot_data trajectory_msgs::JointTrajectory composeTrjMessage(std::vector jtpos){ static trajectory_msgs::JointTrajectory trj; static double ang=0.0; diff --git a/src/roboglue_ros/src/roboglue_recorder.cpp b/src/roboglue_ros/src/roboglue_recorder.cpp index 55d85b3..c0d4a72 100644 --- a/src/roboglue_ros/src/roboglue_recorder.cpp +++ b/src/roboglue_ros/src/roboglue_recorder.cpp @@ -262,7 +262,7 @@ int main(int argc, char **argv) { nh.getParam(param_ns+"move_group_name", robot_data.move_group_name); /// set console log level /// - ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); + ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn); /// set spinner rate /// ros::Rate loop_rate(loopRate); diff --git a/src/roboglue_ros/src/roboglue_relay.cpp b/src/roboglue_ros/src/roboglue_relay.cpp index f93075d..a98f734 100644 --- a/src/roboglue_ros/src/roboglue_relay.cpp +++ b/src/roboglue_ros/src/roboglue_relay.cpp @@ -108,7 +108,7 @@ int main(int argc, char **argv) { /// set spinner rate /// ros::Rate loop_rate(loopRate); /// set console log level /// - ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); + ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn); ROS_INFO("Relay Node Started"); /// initialize MQTT client /// mqtt::async_client client(mqtt_topics.mqttHost,""); diff --git a/src/roboglue_ros/src/roboglue_utils.cpp b/src/roboglue_ros/src/roboglue_utils.cpp index 8d5bd26..18b86f1 100644 --- a/src/roboglue_ros/src/roboglue_utils.cpp +++ b/src/roboglue_ros/src/roboglue_utils.cpp @@ -82,6 +82,10 @@ double rad2deg(double ang){ return ang*180/M_PI; } +double deg2rad(double ang){ + return ang*M_PI/180; +} + bool checkJointJump(moveit_msgs::RobotTrajectoryPtr traj, double thresh){ // identify max joint jump for each joint typedef struct{