empieza a funcionar bien el simulador iterativo

include/diff_platform_simulator_alg_node.h

@@ -27,7 +27,7 @@
 
 #include <iri_base_algorithm/iri_base_algorithm.h>
 #include "diff_platform_simulator_alg.h"
-
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 #include <geometry_msgs/PoseWithCovariance.h>
 #include <geometry_msgs/TwistWithCovariance.h>

launch/fake_localization.launch

 <!-- -->
 <launch>
 
-  <group ns="$(optenv ROBOT tibi)">
+ <!-- <group ns="$(optenv ROBOT tibi)">-->
 
     <node name="fake_localization"
           pkg ="fake_localization"
           type="fake_localization"
           respawn="false" >
-      <param name="odom_frame_id"   value="/$(optenv ROBOT tibi)/odom" />
-      <param name="global_frame_id"   value="/map" />
-      <param name="base_frame_id"   value="/$(optenv ROBOT tibi)/base_footprint" />
+      <param name="odom_frame_id"   value="$(optenv ROBOT tibi)/odom" />
+      <param name="global_frame_id"   value="map" />
+      <param name="base_frame_id"   value="$(optenv ROBOT tibi)/base_footprint" />
       <param name="delta_x"   value="0.0" />
       <param name="delta_y"   value="0.0" />
       <param name="delta_yaw" value="0.0" />
@@ -17,6 +17,6 @@
       <remap from="base_pose_ground_truth" to="/$(optenv ROBOT tibi)/segway/odom"/>
     </node>
 
-  </group>
+ <!-- </group>-->
 
 </launch>

launch/map_server.launch

@@ -17,7 +17,7 @@
         type="map_server" 
         machine="nav" 
         args="$(find iri_akp_local_planner)/maps/$(arg map).yaml">
-     <param name="frame_id" value="/map" />
+     <param name="frame_id" value="map" />
      <remap from="/map"
               to="$(optenv ROBOT tibi)/map"/>
   </node>

launch/map_server_local.launch

@@ -14,9 +14,9 @@
         pkg ="map_server" 
         type="map_server" 
         args="$(find iri_akp_local_planner_approaching)/maps/$(arg map).yaml">
-     <param name="frame_id" value="/map" />
+     <param name="frame_id" value="map" />
      <remap from="/map"
-              to="$(optenv ROBOT tibi)/map"/>
+              to="/$(optenv ROBOT tibi)/map"/>
   </node>
   
 </launch>

src/diff_platform_simulator_alg_node.cpp

@@ -8,9 +8,9 @@ DiffPlatformSimulatorAlgNode::DiffPlatformSimulatorAlgNode(void) :
   setRate(50);
   
   this->public_node_handle_.getParam("robot", this->tf_prefix_);
-  this->odom_id_           = "/" + this->tf_prefix_ + "/odom";
-  this->base_link_id_      = "/" + this->tf_prefix_ + "/base_link";
-  this->base_footprint_id_ = "/" + this->tf_prefix_ + "/base_footprint";
+  this->odom_id_           = this->tf_prefix_ + "/odom";
+  this->base_link_id_      = this->tf_prefix_ + "/base_link";
+  this->base_footprint_id_ = this->tf_prefix_ + "/base_footprint";
   this->linearX = 0.0;
   this->angularZ= 0.0;
 
@@ -96,8 +96,20 @@ void DiffPlatformSimulatorAlgNode::mainNodeThread(void)
     pitch_angle=0.1*vt;
   else if(vt<0)
     pitch_angle=-0.1*vt;
-  geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(accum_th);
-  geometry_msgs::Quaternion odom_quat_pitch = tf::createQuaternionMsgFromRollPitchYaw(0.0,pitch_angle,0.0);
+
+ geometry_msgs::Quaternion odom_quat;
+  tf2::Quaternion tf2_odom_quat;
+  tf2_odom_quat.setRPY(0, 0, accum_th);
+  odom_quat = tf2::toMsg(tf2_odom_quat);
+
+geometry_msgs::Quaternion odom_quat_pitch;
+  tf2::Quaternion tf2_odom_quat_pitch;
+  tf2_odom_quat_pitch.setRPY(0.0,pitch_angle,0.0);
+  odom_quat_pitch = tf2::toMsg(tf2_odom_quat_pitch);
+  
+// antes de tf2, ros-melodic=>  
+ // geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(accum_th);
+ // geometry_msgs::Quaternion odom_quat_pitch = tf::createQuaternionMsgFromRollPitchYaw(0.0,pitch_angle,0.0);
 
   this->transform.translation.x += delta_x;
   this->transform.translation.y += delta_y;

src/fake_laser_gen_alg_node.cpp

@@ -59,7 +59,7 @@ void FakeLaserGenAlgNode::init()
   float32[] ranges
 */
 
-  LaserScan_msg_.header.frame_id = "/map";
+  LaserScan_msg_.header.frame_id = "map";
   LaserScan_msg_.angle_min = -2*1.56643295288;
   LaserScan_msg_.angle_max = 2*1.56643295288;
   LaserScan_msg_.angle_increment = 0.00436332309619;