Robotics StackExchange | Archived questions

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

Answers