diff --git a/include/diff_platform_simulator_alg_node.h b/include/diff_platform_simulator_alg_node.h index f199c554713039f035268591bec6dfcb7a10cd15..55137afb22323154e6447b9d23d2c1f1b1b457e2 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 7d5e1ba0e7915fb36f7a19cf77cc632dae19f665..ac1f54b6192e0482fa4efa25c3c91ce9061c4838 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 d7a34e1b37feb54943a62f214d4de8b96d2f9424..ee1acd687d18e216a06ecddd887f151f7c4355ff 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 6c5c09a335a7457cbc538e8838d379122f446501..8e47e80165d82149525a06e37b4ebae396583e05 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 f775c1df1f22fe95e307b702bc8f9d2b0fc19d6a..730e09ab4c50ab87a32b28561aa2897582738c2b 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 62ea5875116376509a55a88615c71df0e26e1688..0fc99357a967dac3d336878a5eec5f422b863c39 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;