Commit 21ab2ea8 authored by Ely Repiso Polo's avatar Ely Repiso Polo
Browse files

empieza a funcionar bien el simulador iterativo

parent e8f38738
......@@ -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>
<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>
......@@ -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>
......
......@@ -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>
......@@ -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;
......
......@@ -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;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment