search

cartographer-project / cartographer_ros

Provides ROS integration for Cartographer.
Apache License 2.0
1.67k stars 1.21k forks source link

ros2-galactic-Can't start a trajectory with initial pose: Trajectory 0 is in 'DELETED' state. #1701

Open iLanceS opened 2 years ago

iLanceS commented 2 years ago

I use cartographer_ros for localization,and something wrong with my work;

I call '/finish_trajectory' to finish Trajectory 0 ,then i call '/start_trajectory' to initial the pose but it response me 'code=3, message="Trajectory 0 is in \'DELETED\' state."), trajectory_id=0'

Is there someone meet the same error,could someone help me out of this,thanks.

here is the detail:

b"requester: making request: cartographer_ros_msgs.srv.FinishTrajectory_Request(trajectory_id=0)\n\nresponse:\ncartographer_ros_msgs.srv.FinishTrajectory

Response(status=cartographer_ros_msgs.msg.StatusResponse(code=0, message='Finished trajectory 0.'))\n\n"

ros2 service call /start_trajectory cartographer_ros_msgs/srv/StartTrajectory "{configuration_directory: "/home/lance/Desktop/ramdisk/minion_stack_ros2/minion_cartographer/config", configuration_basename: "minion_localization_only.lua", use_initial_pose: "true", initial_pose:  {position: {x: 1.758671522140503, y: -3.28525710105896, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.42142685370252597, w: 0.906862396937038}}, relative_to_trajectory_id: 0}"

b'requester: making request: cartographer_ros_msgs.srv.StartTrajectory_Request(configuration_directory=\'/home/lance/Desktop/ramdisk/minion_stack_ros2/minion_cartographer/config\', configuration_basename=\'minion_localization_only.lua\', use_initial_pose=True, initial_pose=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=1.758671522140503, y=-3.28525710105896, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.42142685370252597, w=0.906862396937038)), relative_to_trajectory_id=0)

response:\ncartographer_ros_msgs.srv.StartTrajectory_Response(status=cartographer_ros_msgs.msg.StatusResponse(code=3, message="Trajectory 0 is in \'DELETED\' state."), trajectory_id=0)\n\n'

here is my lua file:

-- Copyright 2016 The Cartographer Authors
--
-- Licensed under the Apache License, Version 2.0 (the "License");
-- you may not use this file except in compliance with the License.
-- You may obtain a copy of the License at
--
--      http://www.apache.org/licenses/LICENSE-2.0
--
-- Unless required by applicable law or agreed to in writing, software
-- distributed under the License is distributed on an "AS IS" BASIS,
-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-- See the License for the specific language governing permissions and
-- limitations under the License.

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,           
  trajectory_builder = TRAJECTORY_BUILDER,  

  map_frame = "map",                       
  tracking_frame = "base_link",            
  published_frame = "base_link",      
  odom_frame = "odom",                    
  provide_odom_frame = true,             

  publish_frame_projected_to_2d = false,  

  use_odometry = false,                     
  use_nav_sat = false,                     
  use_landmarks = false,                 
  num_laser_scans = 1,                    
  num_multi_echo_laser_scans = 0,           
  num_subdivisions_per_laser_scan = 1,      
  num_point_clouds = 0,                 

  lookup_transform_timeout_sec = 0.2,    
  submap_publish_period_sec = 0.3,         
  pose_publish_period_sec = 2e-2,        
  trajectory_publish_period_sec = 30e-3,

  rangefinder_sampling_ratio = 1.,        
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 100.
TRAJECTORY_BUILDER_2D.min_z = 0.2
--TRAJECTORY_BUILDER_2D.max_z = 1.4
--TRAJECTORY_BUILDER_2D.voxel_filter_size = 0.02

--TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_length = 0.5
--TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.min_num_points = 200.
--TRAJECTORY_BUILDER_2D.adaptive_voxel_filter.max_range = 50.

--TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.max_length = 0.9
--TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.min_num_points = 100
--TRAJECTORY_BUILDER_2D.loop_closure_adaptive_voxel_filter.max_range = 50.
I use cartographer_ros for **localization**,and something wrong with my work;

I call '/finish_trajectory'  to finish Trajectory 0 ,then i call '/start_trajectory' to **initial the pose** but it response me '**code=3, message="Trajectory 0 is in \'DELETED\' state."), trajectory_id=0**'

Is there someone meet the same error,could someone help me out of this,thanks.

### here is the detail:
```bash
b"requester: making request: cartographer_ros_msgs.srv.FinishTrajectory_Request(trajectory_id=0)\n\nresponse:\ncartographer_ros_msgs.srv.FinishTrajectory

Response(status=cartographer_ros_msgs.msg.StatusResponse(code=0, message='Finished trajectory 0.'))\n\n"

ros2 service call /start_trajectory cartographer_ros_msgs/srv/StartTrajectory "{configuration_directory: "/home/lance/Desktop/ramdisk/minion_stack_ros2/minion_cartographer/config", configuration_basename: "minion_localization_only.lua", use_initial_pose: "true", initial_pose:  {position: {x: 1.758671522140503, y: -3.28525710105896, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.42142685370252597, w: 0.906862396937038}}, relative_to_trajectory_id: 0}"

b'requester: making request: cartographer_ros_msgs.srv.StartTrajectory_Request(configuration_directory=\'/home/lance/Desktop/ramdisk/minion_stack_ros2/minion_cartographer/config\', configuration_basename=\'minion_localization_only.lua\', use_initial_pose=True, initial_pose=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=1.758671522140503, y=-3.28525710105896, z=0.0), orientation=geI use cartographer_ros for **localization**,and something wrong with my work;

I call '/finish_trajectory'  to finish Trajectory 0 ,then i call '/start_trajectory' to **initial the pose** but it response me '**code=3, message="Trajectory 0 is in \'DELETED\' state."), trajectory_id=0**'

Is there someone meet the same error,could someone help me out of this,thanks.

### here is the detail:
```bash
b"requester: making request: cartographer_ros_msgs.srv.FinishTrajectory_Request(trajectory_id=0)\n\nresponse:\ncartographer_ros_msgs.srv.FinishTrajectory

Response(status=cartographer_ros_msgs.msg.StatusResponse(code=0, message='Finished trajectory 0.'))\n\n"

ros2 service call /start_trajectory cartographer_ros_msgs/srv/StartTrajectory "{configuration_directory: "/home/lance/Desktop/ramdisk/minion_stack_ros2/minion_cartographer/config", configuration_basename: "minion_localization_only.lua", use_initial_pose: "true", initial_pose:  {position: {x: 1.758671522140503, y: -3.28525710105896, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.42142685370252597, w: 0.906862396937038}}, relative_to_trajectory_id: 0}"

b'requester: making request: cartographer_ros_msgs.srv.StartTrajectory_Request(configuration_directory=\'/home/lance/Desktop/ramdisk/minion_stack_ros2/minion_cartographer/config\', configuration_basename=\'minion_localization_only.lua\', use_initial_pose=True, initial_pose=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=1.758671522140503, y=-3.28525710105896, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.42142685370252597, w=0.906862396937038)), relative_to_trajectory_id=0)

response:\ncartographer_ros_msgs.srv.StartTrajectory_Response(status=cartographer_ros_msgs.msg.StatusResponse(code=3, message="Trajectory 0 is in \'DELETED\' state."), trajectory_id=0)\n\n'

ometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.42142685370252597, w=0.906862396937038)), relative_to_trajectory_id=0)

response:\ncartographer_ros_msgs.srv.StartTrajectory_Response(status=cartographer_ros_msgs.msg.StatusResponse(code=3, message="Trajectory 0 is in \'DELETED\' state."), trajectory_id=0)\n\n'


TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true 
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 1.
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 4e2
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.translation_weight = 2e2
--TRAJECTORY_BUILDER_2D.ceres_scan_matcher.ceres_solver_options.max_num_iterations = 12

--TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.1
--TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = 0.004
--TRAJECTORY_BUILDER_2D.imu_gravity_time_constant = 1.

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 80.
TRAJECTORY_BUILDER_2D.submaps.grid_options_2d.resolution = 0.1
-- TRAJECTORY_BUILDER_2D.pose_extrapolator=true --Lance

POSE_GRAPH.optimize_every_n_nodes = 160.
POSE_GRAPH.constraint_builder.sampling_ratio = 0.7
POSE_GRAPH.constraint_builder.max_constraint_distance = 100.
POSE_GRAPH.constraint_builder.min_score = 0.48
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.60

TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35
-- 100
TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.hit_probability = 0.55
TRAJECTORY_BUILDER_2D.submaps.range_data_inserter.probability_grid_range_data_inserter.miss_probability = 0.49

TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(120)

TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3.

POSE_GRAPH.global_constraint_search_after_n_seconds = 3

return options

include "lcartographer.lua"

-- -- TRAJECTORY_BUILDER.pure_localization = false
TRAJECTORY_BUILDER.pure_localization_trimmer = {
    max_submaps_to_keep = 3,
}

return options
weihaoysgs commented 2 years ago

i have the same problem, do you have solve it ?