GPS waypoints with navsat_transform_node + move_base + navigation stack
Hi to all,
I have a skid-steering wheeled robot with RTK GPS, a SICK laser and a IMU which outputs orientation data. Thanks to the support of @Tom-moore, i was able to correctly setup the odometry (GPS+encoders+IMU). Now, I would like to send a PoseStamped message with move_base in order to implement a GPS waypoint navigation.
I tried to simply run the command:
rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped '{header: {stamp: now, frame_id: "map"}, pose: osition: {x: 4.95, y: -13, z: 0.0}, orientation: {w: 1.0}}}'
but the robot is not moving in RVIZ.
I think that move_base
should be used on a map, is it correct?
Do I need do build a map by using gmapping or similar ROS packages like AMCL?
I'm sorry for these beginner questions, but I'm not able to correctly understand how to build a GPS waypoints navigation.
If I need to use gmapping, how can I use my odometry data?
I tried to do this:
rosrun gmapping slam_gmapping scan:=scan
rosrun rviz rviz
roslaunch husky-gps1.launch
but I see the points of the map in RVIZ jumping and shifting without any logical reason. If I do not launch the gmapping node, then the robot path showed in RVIZ is correct.
What is my mistake?
This is my bag file (32 MB) which contains these topics:
rosbag info test4.bag
path: test4.bag
version: 2.0
duration: 3:09s (189s)
start: May 12 2017 16:40:21.09 (1494600021.09)
end: May 12 2017 16:43:30.81 (1494600210.81)
size: 31.3 MB
messages: 19139
compression: none [42/42 chunks]
types: nav_msgs/Odometry [cd5e73d190d741a2f92e81eda573aca7]
sensor_msgs/Imu [6a62c6daae103f4ff57a132d6f95cec2]
sensor_msgs/LaserScan [90c7ef2dc6895d81024acba2ac42f369]
sensor_msgs/NavSatFix [2d3a8cd499b9b4a0249fb98fd05cfa48]
topics: /fix 190 msgs : sensor_msgs/NavSatFix
/husky_velocity_controller/odom 1897 msgs : nav_msgs/Odometry
/imu/data 7590 msgs : sensor_msgs/Imu
/scan 9462 msgs : sensor_msgs/LaserScan
This is my launch file for the ekf_localization_node
and navsat_transform_node
.
<launch>
<rosparam command="load" file="$(find husky_control)/config/control.yaml" />
<node name="base_controller_spawner" pkg="controller_manager" type="spawner" args="husky_joint_publisher husky_velocity_controller --shutdown-timeout 3"/>
<node pkg="rosbag" type="play" name="rosbag_play" output="screen" args="--clock /home/rocco/Desktop/gps/test1.bag -d 3"/>
<node pkg="tf2_ros" type="static_transform_publisher" name="bl_imu" args="0 0 0 0 0 0 base_link base_imu" />
<node pkg="tf2_ros" type="static_transform_publisher" name="bl_gps" args="0 0 0.4 0 0 0 base_link gps" />
<node pkg="tf2_ros" type="static_transform_publisher" name="bl_laser" args="0.4 0 0.2 0 0 0 base_link laser" />
<!-- Local (odom) instance -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_local" clear_params="true">
<param name="frequency" value="10"/>
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="false"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
<param name="transform_time_offset" value="0.0"/>
<param name="odom0" value="/husky_velocity_controller/odom"/>
<param name="imu0" value="/imu/data"/>
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, true, true,
false, false, true,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="odom0_relative" value="false"/>
<param name="imu0_relative" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="print_diagnostics" value="true"/>
<param name="odom0_queue_size" value="10"/>
<param name="imu0_queue_size" value="10"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug_ekf_localization.txt"/>
<remap from="/odometry/filtered" to="/odometry/filtered/local"/>
</node>
<!-- Global (map) instance -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_global" clear_params="true">
<param name="frequency" value="10"/>
<param name="sensor_timeout" value="0.1"/>
<param name="two_d_mode" value="false"/>
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="map"/>
<param name="transform_time_offset" value="0.0"/>
<param name="odom0" value="/husky_velocity_controller/odom"/>
<param name="odom1" value="/odometry/gps"/>
<param name="imu0" value="/imu/data"/>
<rosparam param="odom0_config">[false, false, false,
false, false, false,
true, true, true,
false, false, true,
false, false, false]</rosparam>
<rosparam param="odom1_config">[true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]</rosparam>
<rosparam param="imu0_config">[false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]</rosparam>
<param name="odom0_differential" value="false"/>
<param name="odom1_differential" value="false"/>
<param name="imu0_differential" value="false"/>
<param name="odom0_relative" value="false"/>
<param name="odom1_relative" value="false"/>
<param name="imu0_relative" value="false"/>
<param name="imu0_remove_gravitational_acceleration" value="true"/>
<param name="print_diagnostics" value="true"/>
<param name="odom0_queue_size" value="10"/>
<param name="odom1_queue_size" value="10"/>
<param name="imu0_queue_size" value="10"/>
<param name="debug" value="false"/>
<param name="debug_out_file" value="debug_ekf_localization.txt"/>
<remap from="/odometry/filtered" to="/odometry/filtered/global"/>
</node>
<!-- navsat_transform -->
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" respawn="true" output="screen">
<param name="frequency" value="10"/>
<param name="delay" value="3"/>
<param name="magnetic_declination_radians" value="0.0640"/>
<param name="yaw_offset" value="1.570796327"/>
<param name="zero_altitude" value="true"/>
<param name="broadcast_utm_transform" value="true"/>
<param name="publish_filtered_gps" value="true"/>
<param name="use_odometry_yaw" value="false"/>
<param name="wait_for_datum" value="false"/>
<remap from="/odometry/filtered" to="/odometry/filtered/global"/>
<remap from="/gps/fix" to="/fix"/>
<remap from="/imu/data" to="/imu/data"/>
</node>
<node pkg="interactive_marker_twist_server" type="marker_server" name="twist_marker_server" output="screen"/>
<node pkg="twist_mux" type="twist_mux" name="twist_mux">
<rosparam command="load" file="$(find husky_control)/config/twist_mux.yaml" />
<remap from="cmd_vel_out" to="husky_velocity_controller/cmd_vel"/>
</node>
</launch>
Asked by Marcus Barnet on 2017-05-11 09:13:35 UTC
Comments
I updated my question by adding a bag file. I hope someone can help me.
Asked by Marcus Barnet on 2017-05-12 11:06:42 UTC
any suggestions?
Asked by Marcus Barnet on 2017-06-02 10:32:04 UTC
Hi Marcus, have you figured out a solution yet?
Asked by noname on 2022-08-13 19:18:34 UTC
My post is from 2017, unfortunately I can't remember the solution I've found, I'm sorry
Asked by Marcus Barnet on 2022-08-14 02:28:16 UTC
No worries
Asked by noname on 2022-08-14 10:27:02 UTC