From 21ab2ea8de754395776b3d483196eb59b8a1c8d3 Mon Sep 17 00:00:00 2001
From: Ely Repiso Polo <erepiso@iri.upc.edu>
Date: Wed, 23 Jun 2021 12:21:49 +0000
Subject: [PATCH] empieza a funcionar bien el simulador iterativo
---
include/diff_platform_simulator_alg_node.h | 2 +-
launch/fake_localization.launch | 10 +++++-----
launch/map_server.launch | 2 +-
launch/map_server_local.launch | 4 ++--
src/diff_platform_simulator_alg_node.cpp | 22 +++++++++++++++++-----
src/fake_laser_gen_alg_node.cpp | 2 +-
6 files changed, 27 insertions(+), 15 deletions(-)
diff --git a/include/diff_platform_simulator_alg_node.h b/include/diff_platform_simulator_alg_node.h
index f199c55..55137af 100644
--- a/include/diff_platform_simulator_alg_node.h
+++ b/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>
diff --git a/launch/fake_localization.launch b/launch/fake_localization.launch
index 7d5e1ba..ac1f54b 100644
--- a/launch/fake_localization.launch
+++ b/launch/fake_localization.launch
@@ -1,15 +1,15 @@
<!-- -->
<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>
diff --git a/launch/map_server.launch b/launch/map_server.launch
index d7a34e1..ee1acd6 100644
--- a/launch/map_server.launch
+++ b/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>
diff --git a/launch/map_server_local.launch b/launch/map_server_local.launch
index 6c5c09a..8e47e80 100644
--- a/launch/map_server_local.launch
+++ b/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>
diff --git a/src/diff_platform_simulator_alg_node.cpp b/src/diff_platform_simulator_alg_node.cpp
index f775c1d..730e09a 100644
--- a/src/diff_platform_simulator_alg_node.cpp
+++ b/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;
diff --git a/src/fake_laser_gen_alg_node.cpp b/src/fake_laser_gen_alg_node.cpp
index 62ea587..0fc9935 100644
--- a/src/fake_laser_gen_alg_node.cpp
+++ b/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;
--
GitLab