search

ori-drs / pronto_anymal_example

Example binary running the Pronto state estimator on the ANYmal B robot
24 stars 10 forks source link

Help ! pronto state estimator running on custom robot simulation is not publishing any msg in topics ? #8

Closed shyamnsl closed 3 years ago

shyamnsl commented 4 years ago

Pronto state estimator is not publishing anything in custom robot implementation . I refer this for implementation of my custom robot . Below is the terminal info that i get when I run the pronto estimator with my simulation

`SUMMARY

PARAMETERS

NODES / alaserMapping (aloam_velodyne/alaserMapping) alaserOdometry (aloam_velodyne/alaserOdometry) alom_pose_publisher (pronto_quadra/alom_pose_publisher.py) ascanRegistration (aloam_velodyne/ascanRegistration) base_link_rename (tf/static_transform_publisher) base_olam_publisher (pronto_quadra/base_aloam_publisher.py) fl_link_rename (tf/static_transform_publisher) fovis_trajectory_server (hector_trajectory_server/hector_trajectory_server) fr_link_rename (tf/static_transform_publisher) ground_truth_publisher (pronto_quadra/ground_truth_path.py) hector_trajectory_server (hector_trajectory_server/hector_trajectory_server) hl_link_rename (tf/static_transform_publisher) hr_link_rename (tf/static_transform_publisher) odom_to_map (tf/static_transform_publisher) pronto_quadra_node (pronto_quadra/pronto_quadra_node) pronto_trajectory_server (hector_trajectory_server/hector_trajectory_server) quadra_state_publisher (robot_state_publisher/robot_state_publisher) rviz (rviz/rviz) simple_fusion (fovis_ros/simple_fusion) state_estimator_trajectory_server (hector_trajectory_server/hector_trajectory_server) velodyne_to_camera_init (tf/static_transform_publisher) world_to_map (tf/static_transform_publisher)

ROS_MASTER_URI=http://localhost:11311

process[quadra_state_publisher-1]: started with pid [15653] process[rviz-2]: started with pid [15654] process[pronto_trajectory_server-3]: started with pid [15659] [ WARN] [1604655376.103838802]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF. process[base_link_rename-4]: started with pid [15662] process[fl_link_rename-5]: started with pid [15669] process[fr_link_rename-6]: started with pid [15674] process[hl_link_rename-7]: started with pid [15677] process[hr_link_rename-8]: started with pid [15686] [ INFO] [1604655376.227601207]: Waiting for tf transform data between frames /odom and base_pronto to become available process[odom_to_map-9]: started with pid [15692] process[world_to_map-10]: started with pid [15697] process[velodyne_to_camera_init-11]: started with pid [15703] process[base_olam_publisher-12]: started with pid [15713] process[alom_pose_publisher-13]: started with pid [15717] process[ground_truth_publisher-14]: started with pid [15718] process[hector_trajectory_server-15]: started with pid [15721] process[pronto_quadra_node-16]: started with pid [15722] process[simple_fusion-17]: started with pid [15727] process[state_estimator_trajectory_server-18]: started with pid [15728] process[fovis_trajectory_server-19]: started with pid [15735] process[ascanRegistration-20]: started with pid [15744] process[alaserOdometry-21]: started with pid [15748] Robot quadra, InverseDynamics::InverseDynamics() Compiled with Eigen debug active process[alaserMapping-22]: started with pid [15750] [ StanceEst ] Mode: THRESHOLD [ StanceEst ] Force threshold: 82 [ KSE ] Mode changed to: 2 [ KSE ] Impact sigma [ KSE ] Weighted avg [ INFO] [1604655376.681997008, 2.893000000]: Waiting for tf transform data between frames /map and /loam_base to become available [ERROR] [1604655376.747675993]: "imu_link" passed to lookupTransform argument target_frame does not exist. [ INFO] [1604655376.815596586, 2.898000000]: Waiting for tf transform data between frames /odom and /base to become available 2 is which_vo_options [ INFO] [1604655376.870834553, 2.901000000]: Waiting for tf transform data between frames /odom and base_fovis to become available 0 is orientation_fusion_mode 1 is initial_pose_mode /home/shyamsrinivasan/auxilary_pkg_build/src/state-estimation/fovis_ros/fovis_ros/config/robot/quadra.yaml is config_filename [full path] output_tf_frame: publish odom to base_fovis Write pose to file: false Received Body to Camera rotation [RPY, deg]: 78.2498 -180 88.603 0.371 0.018 0.162 [B_r_BC] -0.541813 0.555213 -0.451611 0.44071 B_q_BC [xyzw] -0.541813 0.555213 -0.451611 0.44071 B_q_BC [xyzw] Loading stereo configuration0 Focal_length_x: 424.296 Loading stereo configuration1 Focal_length_x: 424.296 Stereo baseline: 0.05 libGL error: failed to create drawable [ WARN] [1604655377.852336274, 3.008000000]: time Map corner and surf num are not enough [ INFO] [1604655380.847128656, 3.353000000]: Finished waiting for tf, waited 0.455000 seconds [ INFO] [1604655381.690578345, 3.440000000]: Finished waiting for tf, waited 0.547000 seconds Disparity Filter is ENABLED Depth Filter is DISABLED FusionCore Constructed [ INFO] [1604655382.286523495, 3.502000000]: output_using_imu_time = 0 [ INFO] [1604655382.288328727, 3.502000000]: /camera/left/image_rect is the image_a topic subscription [for stereo] [ INFO] [1604655382.289285108, 3.502000000]: compressed is the image_a_transport transport [ INFO] [1604655382.291070356, 3.502000000]: /camera/right/image_rect is the image_b topic subscription [for stereo] [ INFO] [1604655382.293890992, 3.502000000]: compressed is the image_b_transport transport [ERROR] [1604655382.339033496, 3.508000000]: Tried to advertise a service that is already advertised in this node [/simple_fusion/compressed/set_parameters] [ INFO] [1604655382.374125410, 3.512000000]: StereoOdom Constructed IMU callback: initializing pose using IMU (roll and pitch) Initialized pose from Transform t [m]: 0 0 0 rpy [deg]: 0.119841, 0.788585, 1.48442 [ INFO] [1604655382.808334009, 3.575000000]: VO frame received [0] [ INFO] [1604655382.809523142, 3.575000000]: image_a [mono8] [ INFO] [1604655382.811238438, 3.575000000]: Second Image Encoding: mono8 WxH: 848 480 utime_prev is zero [at init] no matches, will not publish or output [ INFO] [1604655383.890159196, 3.703000000]: Finished waiting for tf, waited 0.802000 seconds [ DataLogger ] Request to create prontopos.txt [ DataLogger ] Setting path to: /home/shyamsrinivasan/dls_logs/prontopos.txt [ DataLogger ] Start from zero: false [ DataLogger ] Is first time: true [ DataLogger ] Request to create prontovel.txt [ DataLogger ] Setting path to: /home/shyamsrinivasan/dls_logs/prontovel.txt [ DataLogger ] Start from zero: false [ DataLogger ] Is first time: true [ DataLogger ] Request to create velsigma.txt [ DataLogger ] Setting path to: /home/shyamsrinivasan/dls_logs/velsigma.txt [ DataLogger ] Start from zero: false [ DataLogger ] Is first time: true [ INFO] [1604655385.651400536, 3.903000000]: Publishing TF with frame_id: "odom" and child_frame_id: "base_pronto" [ INFO] [1604655385.671052334, 3.903000000]: Sensor id: bias_lock [ INFO] [1604655385.671192922, 3.903000000]: Roll forward: yes [ INFO] [1604655385.671256147, 3.903000000]: Publish head: no [ INFO] [1604655385.671298394, 3.903000000]: Topic: /sensors/imu bias_lock subscribing to /sensors/imu with MsgT = sensormsgs::Imu<std::allocator > [ INFO] [1604655385.680336268, 3.903000000]: bias_lock subscribing to /joint_states with SecondaryMsgT = sensormsgs::JointState<std::allocator > [ VisualOdometryModule ] Covariance: 1e-06 0 0 0 0 0 0 1e-06 0 0 0 0 0 0 4e-06 0 0 0 0 0 0 0.121847 0 0 0 0 0 0 0.121847 0 0 0 0 0 0 0.000304617 [ INFO] [1604655385.693565792, 3.903000000]: Sensor id: fovis [ INFO] [1604655385.693621653, 3.903000000]: Roll forward: yes [ INFO] [1604655385.693650176, 3.903000000]: Publish head: no [ INFO] [1604655385.693683328, 3.903000000]: Topic: /fovis/delta_vo fovis subscribing to /fovis/delta_vo with MsgT = prontomsgs::VisualOdometryUpdate<std::allocator > [ERROR] [1604655385.728588407, 3.903000000]: "imu_link" passed to lookupTransform argument target_frame does not exist. [ INFO] [1604655394.793469107, 4.903000000]: num_to_init: 500 [ INFO] [1604655394.802050976, 4.904000000]: accel_bias_initial: 0 0 0 [ INFO] [1604655394.802976902, 4.904000000]: gyro_bias_initial: 0 0 0 [ INFO] [1604655394.821648396, 4.906000000]: Sensor id: ins [ INFO] [1604655394.821719564, 4.906000000]: Roll forward: yes [ INFO] [1604655394.821747439, 4.906000000]: Publish head: yes [ INFO] [1604655394.821771774, 4.906000000]: Topic: /sensors/imu ins subscribing to /sensors/imu with MsgT = sensormsgs::Imu<std::allocator > [ INFO] [1604655394.824763812, 4.907000000]: Sensor init id: ins [ INFO] [1604655394.824821844, 4.907000000]: Topic: /sensors/imu ins subscribing to /sensors/imu with MsgT = sensormsgs::Imu<std::allocator > [ INFO] [1604655394.835946563, 4.908000000]: Sensor id: legodo [ INFO] [1604655394.836137905, 4.908000000]: Roll forward: yes [ INFO] [1604655394.836212779, 4.908000000]: Publish head: no [ INFO] [1604655394.836272204, 4.908000000]: Topic: /joint_states legodo subscribing to /joint_states with MsgT = sensormsgs::JointState<std::allocator > [ INFO] [1604655394.852214341, 4.909000000]: Sensor init id: pose_meas [ INFO] [1604655394.852323192, 4.909000000]: Topic: /state_estimator/pose_in_odom pose_meas subscribing to /state_estimator/pose_in_odom with MsgT = geometrymsgs::PoseWithCovarianceStamped<std::allocator > [ INFO] [1604655447.853607630, 10.601000000]: VO frame received [120] [ INFO] [1604655511.551131209, 17.210000000]: VO frame received [240] [ WARN] [1604655539.261998271, 20.012000000]: No transform between frames /odom and base_pronto available after 20.012000 seconds of waiting. This warning only prints once. [ INFO] [1604655574.758592694, 23.694000000]: VO frame received [360]

`

shyamnsl commented 3 years ago

/state_estimator/pose_in_odom is topic were pronto subscribes to initialise its filter . Try to pass some information regarding initial state of the robot to this topic.