Launch mission without runtime manager hmi

Autoware
1

Hi everyone,

I am developing a simple HMI for launch a mission, this HMI is in a laptop and the processing PC is on real car. I want launch the same nodes that I launch with the original HMI but something is missing.

I do the next steps:

  • Start gps and Velodyne
  • rosparam set localizer velodyne
  • run node /base_link_to_localizer with velodyne pose
  • load vehicle_model and vehicle_info
  • load pointcloud map
  • run node /world_to_map
  • run node /voxel_grid
  • run node /nmea2tf
  • run node /ndt_matching
  • run nodes of vel_to_pose_connect

In original HMI, with all these nodes running you can see in left panel that tf tree is correct, but in my case there are multiple warning in the transform of velodyne to world.

I have executed rqt_tf_tree and I can see two tf unconnected one between velodyne - base_link and other between world - map - gps

What more I have to launch?

Any help is welcome

PD: I have in laptop .launch files set the machine name, IP, … etc, I can connect using ssh and I have checked sync issues.

2

@javcursor if the tf tree is not connected. I suspect ndt matching is missing an initial pose estimate. Usually that is given using rviz initial pose button. But since you’re using only terminal commands you’ll need to provide it as a message. Publish a geometry_msgs::PoseWithCovarianceStamped message type in the initialpose topic with the initial pose of your localizer (lidar) in the map. You might use rostopic to achieve that.

3

thanks for the hint, I’ve been doing tests and I’ve seen that this particular topic is empty, but I was missing all /config /… topics.
However I still can not get it to work, I isolated the error and it occurs when launching the ndt_matching

jcs@PC1:~$ roslaunch lidar_localizer ndt_matching.launch 
... logging to /home/jcs/.ros/log/1134959a-5077-11e9-9a93-7062b8945ad9/roslaunch-PC1-24173.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://192.168.20.110:40779/

SUMMARY
========

PARAMETERS
 * /ndt_matching/get_height: False
 * /ndt_matching/imu_topic: /imu_raw
 * /ndt_matching/imu_upside_down: False
 * /ndt_matching/method_type: 0
 * /ndt_matching/offset: linear
 * /ndt_matching/queue_size: 1
 * /ndt_matching/use_gnss: 1
 * /ndt_matching/use_imu: False
 * /ndt_matching/use_local_transform: False
 * /ndt_matching/use_odom: False
 * /rosdistro: kinetic
 * /rosversion: 1.12.14

NODES
  /
    ndt_matching (lidar_localizer/ndt_matching)

ROS_MASTER_URI=http://192.168.20.110:11311

process[ndt_matching-1]: started with pid [24212]
Warning: Invalid argument passed to canTransform argument source_frame in tf2 frame_ids cannot be empty
         at line 126 in /tmp/binarydeb/ros-kinetic-tf2-0.5.20/src/buffer_core.cpp
Warning: Invalid argument passed to canTransform argument source_frame in tf2 frame_ids cannot be empty
         at line 126 in /tmp/binarydeb/ros-kinetic-tf2-0.5.20/src/buffer_core.cpp
Warning: Invalid argument passed to canTransform argument source_frame in tf2 frame_ids cannot be empty
         at line 126 in /tmp/binarydeb/ros-kinetic-tf2-0.5.20/src/buffer_core.cpp
Warning: Invalid argument passed to canTransform argument source_frame in tf2 frame_ids cannot be empt


jcs@PC1:~$ rostopic echo /config/ndt 
header: 
  seq: 2
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: ''
init_pos_gnss: 0
x: 45.0
y: 0.0
z: 0.0
roll: 0.0
pitch: -0.0121999997646
yaw: 0.0
use_predict_pose: 0
error_threshold: 0.10000000149
resolution: 1.0
step_size: 0.10000000149
trans_epsilon: 0.00999999977648
max_iterations: 30

I can not know what frame_id I’m missing

@amc-nu Edit with new info, full .launch file, I have to had something more, but just should be sufficient to get localization.

      <launch>
 <group>
  <machine name="ProcPC" address="192.168.20.110" user="greendog" env-loader="/home/greendog/Autoware/ros/devel/env.sh" default="true" timeout="10" />
<!-- load PCD -->
   <arg name="area" default="noupdate" />
   <node pkg="map_file" type="points_map_loader" name="points_map_loader" args="$(arg area) /home/greendog/AutowareData/CircuitMap/pointcloud/Circuito.pcd" /><!-- Lanza el launch del GPS Trimble -->
<!--    <include file="$(find nmea_navsat)/scripts/nmea_navsat.launch"> -->
    <arg name="port_GPS" value="/dev/ttyS1" />
    <arg name="baud_GPS" value="115200" />
<!--    </include> -->
    
    <node pkg="nmea_navsat_driver" type="nmea_topic_serial_reader" name="nmea_topic_serial_reader" output="screen" >
    <param name="port" value="$(arg port_GPS)" />
    <param name="baud" value="$(arg baud_GPS)" />
    </node>
    <node pkg="nmea_navsat_driver" type="nmea_topic_driver" name="nmea_topic_driver" />

<!-- Lanza el nodo de la IMU   -->
    <arg name="port_IMU" value="/dev/ttyS0" />
    <arg name="baud_IMU" value="38400"/>
    <node pkg="memsic_imu" type="vg440_node" name="vg440_node" >
    <param name="port" value="$(arg port_IMU)" />
    <param name="baudrate" value="$(arg baud_IMU)" />
    </node>

<!-- Convierte el quaternion de la IMU a Euler -->
    <node pkg="topic_tools" type="transform" name="quaternion2euler" args="/imu_raw/orientation /euler geometry_msgs/Vector3 'tf.transformations.euler_from_quaternion([m.x, m.y, m.z, m.w])' --import tf" />

<!-- Lanza el launch del Velodyne -->
 <!--
  <include file="$(find velodyne_pointcloud)/launch/velodyne_vlp16.launch">
  <arg name="calibration" default="/home/greendog/Autoware/ros/src/airportshmi/extras/VLP16db.yaml"/>
  </include>
  -->


  <arg name="device_ip" default="192.168.10.201" />
  <arg name="frame_id" default="velodyne" />
  <arg name="manager" default="$(arg frame_id)_nodelet_manager" />
  <arg name="port" default="2368" />
  <arg name="read_fast" default="false" />
  <arg name="read_once" default="false" />
  <arg name="repeat_delay" default="0.0" />
  <arg name="rpm" default="600.0" />
  <arg name="cut_angle" default="-0.01" />
  <arg name="pcap" default="" />
  <arg name="calibration" default="/home/greendog/AutowareData/VelodyneCFG/VLP16db.yaml"/>
  <arg name="min_range" default="0.4" />
  <arg name="max_range" default="130.0" />
  <arg name="model" default="VLP16"/>
  <arg name="topic_name_velodyne" default="points_raw"/>

  <!-- start nodelet manager -->
  <node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" />


  <!-- load driver nodelet into it -->
  <node pkg="nodelet" type="nodelet" name="$(arg manager)_driver" args="load velodyne_driver/DriverNodelet $(arg manager)" >
    <param name="device_ip" value="$(arg device_ip)" />
    <param name="frame_id" value="$(arg frame_id)"/>
    <param name="model" value="$(arg model)"/>
    <param name="pcap" value="$(arg pcap)"/>
    <param name="port" value="$(arg port)" />
    <param name="read_fast" value="$(arg read_fast)"/>
    <param name="read_once" value="$(arg read_once)"/>
    <param name="repeat_delay" value="$(arg repeat_delay)"/>
    <param name="rpm" value="$(arg rpm)"/>
    <param name="cut_angle" value="$(arg cut_angle)"/>
  </node> 

  <!-- start cloud nodelet -->
 <node pkg="nodelet" type="nodelet" name="cloud_nodelet" args="load velodyne_pointcloud/CloudNodelet velodyne_nodelet_manager">
    <param name="calibration" value="$(arg calibration)"/>
    <param name="min_range" value="$(arg min_range)"/>
    <param name="max_range" value="$(arg max_range)"/>
    <remap from="velodyne_points" to="$(arg topic_name_velodyne)"/>
  </node>


<!-- Lanza el nodo del veh\Ufffffffflo, villager6_interface -->
  <arg name="wheel_base" default="2.502"/>
  <arg name="mode" default="8"/>
  <arg name="device" default="can0"/>
  <arg name="loop_rate" default="100"/>
  <arg name="stop_time_sec" default="2"/>

  <node pkg="villager" name="villager6_interface" type="villager6_interface" output="log">
    <param name="wheel_base" value="$(arg wheel_base)"/>
    <param name="mode" value="$(arg mode)"/>
    <param name="device" value="$(arg device)"/>
    <param name="loop_rate" value="$(arg loop_rate)"/>
    <param name="stop_time_sec" value="$(arg stop_time_sec)"/>
  </node>

  <rosparam command="load" file="/home/greendog/AutowareData/AutowareExecutiondumpedparams.yaml" />


<!-- base_link_to_localizer -->
   <arg name="x" default="2.2"/>
   <arg name="y" default="0"/>
   <arg name="z" default="1.86"/>
   <arg name="yaw" default="0"/>
   <arg name="pitch" default="-0.0122"/>
   <arg name="roll" default="0"/>
   <arg name="frame_id_baselink2localizer" default="base_link"/>
   <arg name="child_frame_id_baselink2localizer" default="velodyne"/>
   <arg name="period_in_ms" default="10"/>
   <node type="static_transform_publisher" pkg="tf" name="base_link_to_localizer" args="$(arg x) $(arg y) $(arg z) $(arg yaw) $(arg pitch) $(arg roll) $(arg frame_id_baselink2localizer) $(arg child_frame_id_baselink2localizer) $(arg period_in_ms)"/>

<!-- vehicle model -->
   <arg name="base_frame" default="/base_link"/>
   <arg name="topic_name" default="/vehicle_model"/>
   <arg name="offset_x" default="1.2"/>
   <arg name="offset_y" default="0.0"/>
   <arg name="offset_z" default="0.0"/>
   <arg name="offset_roll" default="0.0"/> <!-- degree -->
   <arg name="offset_pitch" default="0.0"/> <!-- degree -->
   <arg name="offset_yaw" default="0.0"/> <!-- degree -->
   <arg name="model_path" default="/home/greendog/AutowareData/Model/VILLAGER_6.urdf" />
   <arg name="gui" default="False" />

   <param name="robot_description" textfile="$(arg model_path)" />
   <param name="use_gui" value="$(arg gui)"/>
   <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

<!-- vehicle info -->
   <arg name="info_path" default="/home/greendog/AutowareData/Vehicle_info/VILLAGER_6.yaml"/>
   <rosparam command="load" file="$(arg info_path)"/>



<!-- world_to_map -->
   <node pkg="map_tf_generator" type="map_tf_generator" name="world_to_map" /> 

<!-- rostopic pub voxel_grid cfg -->
   <node pkg="rostopic" type="rostopic" name="rostopic_voxelgrid" args="pub /config/voxel_grid_filter autoware_config_msgs/ConfigVoxelGridFilter '{voxel_leaf_size: 0.8, measurement_range: 200.0}'" />

<!-- voxel_grid_filter -->
   <arg name="sync" default="false" />
   <arg name="node_name" default="voxel_grid_filter" />
   <arg name="points_topic" default="points_raw" />
   <arg name="output_log" default="false" />
   <node pkg="points_downsampler" name="$(arg node_name)" type="$(arg node_name)">
    <param name="points_topic" value="$(arg points_topic)" />
    <remap from="/points_raw" to="/sync_drivers/points_raw" if="$(arg sync)" />
    <param name="output_log" value="$(arg output_log)" />
   </node>

<!-- nmea2tfpose -->
   <arg name="plane" default="19"/>
   <node pkg="gnss_localizer" type="nmea2tfpose" name="nmea2tfpose" output="screen">
    <param name="plane" value="$(arg plane)"/>
   </node>

<!-- rostopic pub ndt_matching cfg -->
   <node pkg="rostopic" type="rostopic" name="rostopic" args="pub /config/ndt autoware_config_msgs/ConfigNdt '{header: {seq: 0, stamp: {secs: 0, nsecs: 0}, frame_id: 'world'}, init_pos_gnss: 0, x: 45.0, y: 0.0, z: 0.0, roll: 0.0, pitch: -0.0122, yaw: 0.0, use_predict_pose: 0, error_threshold: 0.1, resolution: 1.0, step_size: 0.1, trans_epsilon: 0.01, max_iterations: 30}'" />

<!-- ndt_matching -->
  <arg name="method_type" default="0" /> 
   <arg name="use_gnss" default="0" />
   <arg name="use_odom" default="false" />
   <arg name="use_imu" default="false" />
   <arg name="imu_upside_down" default="false" />
   <arg name="imu_topic" default="/imu_raw" />
   <arg name="queue_size" default="1" />
   <arg name="offset" default="linear" />
   <arg name="get_height" default="false" />
   <arg name="use_local_transform" default="false" />
   <arg name="sync_ndt" default="false" />
   <node pkg="lidar_localizer" type="ndt_matching" name="ndt_matching" output="log">
    <param name="method_type" value="$(arg method_type)" />
    <param name="use_gnss" value="$(arg use_gnss)" />
    <param name="use_odom" value="$(arg use_odom)" />
    <param name="use_imu" value="$(arg use_imu)" />
    <param name="imu_upside_down" value="$(arg imu_upside_down)" />
    <param name="imu_topic" value="$(arg imu_topic)" />
    <param name="queue_size" value="$(arg queue_size)" />
    <param name="offset" value="$(arg offset)" />
    <param name="get_height" value="$(arg get_height)" />
    <param name="use_local_transform" value="$(arg use_local_transform)" />
    <remap from="/points_raw" to="/sync_drivers/points_raw" if="$(arg sync_ndt)" />
   </node>   

<!-- /initialpose geometry_msgs/PoseWithCovarianceStamped '{header: {seq: 0, stamp: {secs: 0, nsecs: 0}, frame_id: 'map'}, pose: {pose: {position: {x: 45.0, y: 0.0, z: 1.86}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0}}, covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}}' " /> -->

<!-- vel_pose_connect -->
   <arg name="topic_pose_stamped" default="" />
   <arg name="topic_twist_stamped" default="" />
   <arg name="sim_mode" default="false" />

   <group unless="$(arg sim_mode)">
    <node pkg="autoware_connector" type="can_status_translator" name="can_status_translator" output="log" />
    <node pkg="topic_tools" type="relay" name="pose_relay" output="log" args="$(arg topic_pose_stamped) /ndt_pose"/>
    <node pkg="topic_tools" type="relay" name="vel_relay" output="log" args="$(arg topic_twist_stamped) /estimate_twist"/>
   </group>

   <group if="$(arg sim_mode)">
    <node pkg="topic_tools" type="relay" name="pose_relay" output="log" args="/sim_pose /current_pose"/>
    <node pkg="topic_tools" type="relay" name="vel_relay" output="log" args="/sim_velocity /current_velocity"/>
   </group>

<!--               MQTT                    -->
  <arg name="use_tls" default="false" />
  <node name="mqtt_bridge" pkg="mqtt_bridge" type="mqtt_bridge_node.py" output="log">
<!--    <rosparam command="delete" param="" /> -->
<!--    <rosparam command="load" file="$(find mqtt_bridge)/config/config/demo_params.yaml" /> -->
    <rosparam command="load" file="/home/greendoog/Autoware/ros/src/mqtt_comms/comms_params.yaml" />
    <rosparam if="$(arg use_tls)" command="load" ns="mqtt" file="$(find mqtt_bridge)/config/tls_params.yaml" />
  </node>


<!-- rostopic pub ring_ground_filter cfg -->
   <node pkg="rostopic" type="rostopic" name="rostopic_ring_ground_filter" args="pub /config/ring_ground_filter autoware_config_msgs/ConfigRingGroundFilter '{sensor_model: '16', sensor_height: 1.86, max_slope: 15.6, vertical_thres: 0.08}'" />

<!-- rostopic pub twist_filter cfg -->
   <node pkg="rostopic" type="rostopic" name="rostopic_twist_filter" args="pub /config/twist_filter autoware_config_msgs/ConfigTwistFilter '{header: {seq: 0, stamp: {secs: 0, nsecs: 0}, frame_id: ''}, omega_limit: 1, lateral_accel_limit: 0.0, lowpass_gain_linear_x: 0.0, lowpass_gain_angular_z: 0.0}'" />

<!-- rostopic pub waypoint follower cfg -->
   <node pkg="rostopic" type="rostopic" name="rostopic_waypoint_follower" args="pub /config/waypoint_loader autoware_config_msgs/ConfigWaypointLoader '{multi_lane_csv: '/home/greendog/Autoware/ros/test3_replanned_20.csv', replanning_mode: True, velocity_max: 20.0, velocity_min: 10.0, accel_limit: 1.5, decel_limit: 5.0, radius_thresh: 30.0, radius_min: 10.0, resample_mode: True, resample_interval: 4.0, velocity_offset: 3, end_point_offset: 4}'" />

<!-- rostopic pub velocity_set cfg -->
   <node pkg="rostopic" type="rostopic" name="rostopic_velocity_set" args="pub /config/velocity_set autoware_config_msgs/ConfigVelocitySet '{header: {seq: 0, stamp: {secs: 0, nsecs: 0}, frame_id: ''}, stop_distance_obstacle: 20.0, stop_distance_stopline: 0.0, detection_range: 3.0, threshold_points: 3, detection_height_top: 0.2, detection_height_bottom: -3.0, deceleration_obstacle: 3.0, deceleration_stopline: 3.0, velocity_change_limit: 25.5, deceleration_range: 1.0, temporal_waypoints_size: 12.0 }'" />
  <!-- rostopic pub initial_pose -->
  <node type="rostopic" pkg="rostopic" args="pub /initialpose geometry_msgs/PoseWithCovarianceStamped '{header: {seq: 0, stamp: {secs: 0, nsecs: 0}, frame_id: ''}, pose: {pose: {position: {x: 45.0, y: 0.0, z: 1.86}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0}}, covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}}' " name="rostopic_initial_pose" output="log" />

 </group>
</launch>
4

Solved, I have putted the same position in topics /initialpose and /config/ndt (be carefully because one has Euler angles and other quaternion :exploding_head:), also I have fix errors in vel_pose_connect launch.

5

Thanks for sharing your solution @javcursor
Sorry I couldn’t reach before.

6

:scream_cat::scream_cat::scream_cat: