Prima implementazione dell'homing, i giunti vanno tutti a 0

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<std::string> joint_names;
-    std::map<double> joint_homes;
+    std::map<std::string, double> 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);

src/roboglue_ros/src/roboglue_follower.cpp

@@ -22,12 +22,12 @@
 //////////////////////////////////////////
 void testFunction( ros::Publisher* trj_pub);
 trajectory_msgs::JointTrajectory composeTrjMessage(std::vector<double> 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<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]));
+                }
+                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<std_msgs::String>(ros_topics.commandSub, static_cast<uint32_t>(msgBufferSize),
-                                                                 boost::bind(commandCallback, _1, &is, &pos_data));
+                                                                 boost::bind(commandCallback, _1, &is, &publishers, &pos_data, &robot_data));
     ros::Subscriber coordinate_sub = nh.subscribe<std_msgs::String>(ros_topics.coordSub, static_cast<uint32_t>(msgBufferSize),
                                                                     boost::bind(coordinateCallback, _1, &publishers, &pos_data));
     ros::Subscriber state_sub = nh.subscribe<std_msgs::String>(ros_topics.stateSub, static_cast<uint32_t>(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
+void resetModelJointsHome(robotData* rdata) {
-bool resetModelJointsHome(geometry_msgs::Pose home_pos, bool execute=false) {
     ROS_DEBUG("Setting joint position to home");
-    if (execute){
+    std::map<std::string, double> jhome = rdata->joint_homes;
-        return false;
+    for (auto joint : jhome) {
-    } else {
+        std::string jname = joint.first;
-        return false;
+        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<double> jtpos){
     static trajectory_msgs::JointTrajectory trj;
     static double ang=0.0;

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);
 

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,"");

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{