piccole modifiche

Tutte le posizioni di default fanno parte del file .yaml

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<std::string, double> joint_homes;
+    std::map<std::string, float> 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<std::string, double> floatMap2doubleMap(std::map<std::string,float>);
 double e3dist(geometry_msgs::Twist, geometry_msgs::Twist);
 double rad2deg(double ang);
 double deg2rad(double ang);

src/roboglue_ros/launch/test.launch

@@ -4,4 +4,5 @@
     <!--    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${node}] [${function}]: ${message}" />-->
 
     <rosparam command="load" file="$(find roboglue_ros)/config/jointDefaults.yaml" />
+    <node name="roboglue_ros_test" pkg="roboglue_ros" type="roboglue_ros_test" output="screen" required="true" />
 </launch>

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<double> jtpos);
-void resetModelJointsHome(robotData*);
+trajectory_msgs::JointTrajectory composeTrjMessage(std::vector<double> jtpos, std::vector<std::string> jnames);
 
 ///////////////////////////////////////////
 ///////// ROS CALLBACKS //////////////////
@@ -47,20 +46,18 @@ 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);
-            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);
+            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]));
             }
-                pubs->at("trj_pub")->publish(composeTrjMessage(jPos));
+            if (!params["execute"].get<bool>()){
+                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!!");
             }
@@ -73,7 +70,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 +123,7 @@ void timedPublishCallback(const ros::TimerEvent&, internalState* is, publisherMa
     double timeout = 0.1;
     uint attempts = 10;
     bool foundIK = false;
-    std::vector<double> jPos, jStartPos = {M_PI/2, -3*M_PI/4, +3*M_PI/4, -M_PI/2, -M_PI/2, 0};
+    std::vector<double> jPos;
 
     if (is->isRealtime){
         twPose = pos->twist;
@@ -141,17 +138,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 +157,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 +259,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<double> jtpos){
+trajectory_msgs::JointTrajectory composeTrjMessage(std::vector<double> jtpos, std::vector<std::string> 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);

src/roboglue_ros/src/roboglue_test.cpp

@@ -21,7 +21,21 @@ int main(int argc, char **argv)
     spinner.start();
     ros::Publisher trajectory_pub = nh.advertise<trajectory_msgs::JointTrajectory>("/pos_based_pos_traj_controller/command", 1000);
     ROS_INFO("Hello world!");
+    // Construct a map of strings
+    std::map<std::string,std::string> map_s, map_s2;
+    map_s["a"] = "foo";
+    map_s["b"] = "bar";
+    map_s["c"] = "baz";
 
+    // Set and get a map of strings
+    nh.setParam("my_string_map", map_s);
+    nh.getParam("my_string_map", map_s2);
+
+    while(ros::ok) {
+       sleep(5);
+    }
+
+    /*
     //motion group interface
     static const std::string MOVEG = "manipulator";
     moveit::planning_interface::MoveGroupInterface movegroup(MOVEG);
@@ -157,5 +171,5 @@ int main(int argc, char **argv)
 
     smon.stopStateMonitor();
     smon.stopSceneMonitor();
-
+*/
 }

src/roboglue_ros/src/roboglue_utils.cpp

@@ -70,6 +70,14 @@ geometry_msgs::PoseStamped twist2Pose(geometry_msgs::TwistStamped twt){
     return newPoseStamp;
 }
 
+std::map<std::string, double> floatMap2doubleMap(std::map<std::string,float> m) {
+    std::map<std::string, double> tmp;
+    for (auto j : m){
+        tmp[j.first] = (deg2rad(static_cast<double>(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) +