Hi @bshah,
I was trying to run stretch_funmap by following the instructions on README.md. I pressed space thrice to ensure that the robot mapped the region around it before giving any goals.
When I tried to give a goal using 2D Nav goal, I was getting the following error.
1633112175.920392192 WARN /ekf_se [/tmp/binarydeb/ros-melodic-robot-localization-2.6.7/src/ros_filter_utilities.cpp:127(RosFilterUtilities::lookupTransformSafe)] [topics: /rosout, /diagnostics, /tf, /odometry/filtered] Transform from imu_mobile_base to base_link was unavailable for the time requested. Using latest instead.
1633112177.342968966 INFO /rviz [/tmp/binarydeb/ros-melodic-rviz-1.13.13/src/rviz/default_plugin/tools/goal_tool.cpp:84(GoalTool::onPoseSet)] [topics: /rosout, /initialpose, /move_base_simple/goal, /clicked_point] Setting goal: Frame:map, Position(-0.458, -0.199, 0.000), Orientation(0.000, 0.000, 0.975, -0.224) = Angle: -2.690
1633112177.345457077 INFO /funmap [funmap:1165(FunmapNode.navigate_to_goal_topic_callback)] [topics: /funmap/point_cloud2, /stretch/joint_states, /stretch_controller/follow_joint_trajectory/result, /move_base/feedback, /rosout, /tf, /clicked_point, /move_base/goal, /initialpose, /funmap/obstacle_point_cloud2, /stretch_controller/follow_joint_trajectory/feedback, /move_base/status, /move_base_simple/goal, /stretch_controller/follow_joint_trajectory/goal, /stretch_controller/follow_joint_trajectory/cancel, /funmap/voi_marker, /funmap/marker_array, /tf_static, /move_base/result, /camera/depth/color/points, /funmap/navigation_plan_markers, /stretch_controller/follow_joint_trajectory/status, /move_base/cancel] Navigate to goal simple navigate to goal topic received a command!
1633112177.348592996 INFO /funmap [funmap:1166(FunmapNode.navigate_to_goal_topic_callback)] [topics: /funmap/point_cloud2, /stretch/joint_states, /stretch_controller/follow_joint_trajectory/result, /move_base/feedback, /rosout, /tf, /clicked_point, /move_base/goal, /initialpose, /funmap/obstacle_point_cloud2, /stretch_controller/follow_joint_trajectory/feedback, /move_base/status, /move_base_simple/goal, /stretch_controller/follow_joint_trajectory/goal, /stretch_controller/follow_joint_trajectory/cancel, /funmap/voi_marker, /funmap/marker_array, /tf_static, /move_base/result, /camera/depth/color/points, /funmap/navigation_plan_markers, /stretch_controller/follow_joint_trajectory/status, /move_base/cancel] header:
seq: 0
stamp:
secs: 1633112177
nsecs: 342865281
frame_id: "map"
pose:
position:
x: -0.45775449276
y: -0.199394524097
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.974642767443
w: -0.223766565578
1633112177.350264072 INFO /funmap [funmap:546(FunmapNode.pose_to_map_pixel)] [topics: /funmap/point_cloud2, /stretch/joint_states, /stretch_controller/follow_joint_trajectory/result, /move_base/feedback, /rosout, /tf, /clicked_point, /move_base/goal, /initialpose, /funmap/obstacle_point_cloud2, /stretch_controller/follow_joint_trajectory/feedback, /move_base/status, /move_base_simple/goal, /stretch_controller/follow_joint_trajectory/goal, /stretch_controller/follow_joint_trajectory/cancel, /funmap/voi_marker, /funmap/marker_array, /tf_static, /move_base/result, /camera/depth/color/points, /funmap/navigation_plan_markers, /stretch_controller/follow_joint_trajectory/status, /move_base/cancel] clicked_image_pixel =[1.25504092e+03 1.36456575e+03 1.17627119e+01 1.00000000e+00]
1633112177.351090908 INFO /funmap [funmap:548(FunmapNode.pose_to_map_pixel)] [topics: /funmap/point_cloud2, /stretch/joint_states, /stretch_controller/follow_joint_trajectory/result, /move_base/feedback, /rosout, /tf, /clicked_point, /move_base/goal, /initialpose, /funmap/obstacle_point_cloud2, /stretch_controller/follow_joint_trajectory/feedback, /move_base/status, /move_base_simple/goal, /stretch_controller/follow_joint_trajectory/goal, /stretch_controller/follow_joint_trajectory/cancel, /funmap/voi_marker, /funmap/marker_array, /tf_static, /move_base/result, /camera/depth/color/points, /funmap/navigation_plan_markers, /stretch_controller/follow_joint_trajectory/status, /move_base/cancel] end_xy =[1255 1365]
1633112177.960328829 WARN /ekf_se [/tmp/binarydeb/ros-melodic-robot-localization-2.6.7/src/ros_filter_utilities.cpp:127(RosFilterUtilities::lookupTransformSafe)] [topics: /rosout, /diagnostics, /tf, /odometry/filtered] Transform from imu_mobile_base to base_link was unavailable for the time requested. Using latest instead.
1633112179.960523231 WARN /ekf_se [/tmp/binarydeb/ros-melodic-robot-localization-2.6.7/src/ros_filter_utilities.cpp:127(RosFilterUtilities::lookupTransformSafe)] [topics: /rosout, /diagnostics, /tf, /odometry/filtered] Transform from imu_mobile_base to base_link was unavailable for the time requested. Using latest instead.
1633112180.039478063 INFO /d435i_configure [d435i_configure:21(D435iConfigureNode.turn_on_default_mode)] [topics: /camera/stereo_module/parameter_updates, /rosout, /camera/stereo_module/parameter_descriptions] Set D435i to Default mode
1633112180.069231033 INFO /funmap [funmap:710(FunmapNode.navigate_to_map_pixel)] [topics: /funmap/point_cloud2, /stretch/joint_states, /stretch_controller/follow_joint_trajectory/result, /move_base/feedback, /rosout, /tf, /clicked_point, /move_base/goal, /initialpose, /funmap/obstacle_point_cloud2, /stretch_controller/follow_joint_trajectory/feedback, /move_base/status, /move_base_simple/goal, /stretch_controller/follow_joint_trajectory/goal, /stretch_controller/follow_joint_trajectory/cancel, /funmap/voi_marker, /funmap/marker_array, /tf_static, /move_base/result, /camera/depth/color/points, /funmap/navigation_plan_markers, /stretch_controller/follow_joint_trajectory/status, /move_base/cancel] trigger_result = success: True
message: "Default mode enabled."
1633112180.074440956 INFO /stretch_driver [joint_trajectory_server.py:67(JointTrajectoryAction.execute_cb)] [topics: /stretch_controller/follow_joint_trajectory/feedback, /stretch/joint_states, /stretch_controller/follow_joint_trajectory/result, /stretch_controller/follow_joint_trajectory/status, /rosout, /magnetometer_mobile_base, /stretch_controller/follow_joint_trajectory/goal, /odom, /imu_wrist, /imu_mobile_base, /stretch_controller/follow_joint_trajectory/cancel, /stretch/cmd_vel] /stretch_driver joint_traj action: New trajectory received with joint_names = ['joint_head_tilt', 'joint_head_pan']
1633112180.099986076 INFO /stretch_driver [joint_trajectory_server.py:208(JointTrajectoryAction.success_callback)] [topics: /stretch_controller/follow_joint_trajectory/feedback, /stretch/joint_states, /stretch_controller/follow_joint_trajectory/result, /stretch_controller/follow_joint_trajectory/status, /rosout, /magnetometer_mobile_base, /stretch_controller/follow_joint_trajectory/goal, /odom, /imu_wrist, /imu_mobile_base, /stretch_controller/follow_joint_trajectory/cancel, /stretch/cmd_vel] /stretch_driver joint_traj action: Achieved all target points.
1633112180.462938070 ERROR /funmap [funmap:1178(FunmapNode.navigate_to_goal_topic_callback)] [topics: /funmap/point_cloud2, /stretch/joint_states, /stretch_controller/follow_joint_trajectory/result, /move_base/feedback, /rosout, /tf, /clicked_point, /move_base/goal, /initialpose, /funmap/obstacle_point_cloud2, /stretch_controller/follow_joint_trajectory/feedback, /move_base/status, /move_base_simple/goal, /stretch_controller/follow_joint_trajectory/goal, /stretch_controller/follow_joint_trajectory/cancel, /funmap/voi_marker, /funmap/marker_array, /tf_static, /move_base/result, /camera/depth/color/points, /funmap/navigation_plan_markers, /stretch_controller/follow_joint_trajectory/status, /move_base/cancel] **Failed to create distance map and traversable mask.**
1633112182.000340772 WARN /ekf_se [/tmp/binarydeb/ros-melodic-robot-localization-2.6.7/src/ros_filter_utilities.cpp:127(RosFilterUtilities::lookupTransformSafe)] [topics: /rosout, /diagnostics, /tf, /odometry/filtered] Transform from imu_mobile_base to base_link was unavailable for the time requested. Using latest instead.
When I tried hitting the “\ |” key, the robot generated a plan, but the robot didn’t move.
1633113000.628679990 INFO /funmap [navigate.py:213(MoveBase.check_move_state)] [topics: /funmap/point_cloud2, /stretch/joint_states, /stretch_controller/follow_joint_trajectory/result, /move_base/feedback, /rosout, /tf, /clicked_point, /move_base/goal, /initialpose, /funmap/obstacle_point_cloud2, /stretch_controller/follow_joint_trajectory/feedback, /move_base/status, /move_base_simple/goal, /stretch_controller/follow_joint_trajectory/goal, /stretch_controller/follow_joint_trajectory/cancel, /funmap/voi_marker, /funmap/marker_array, /tf_static, /move_base/result, /camera/depth/color/points, /funmap/navigation_plan_markers, /stretch_controller/follow_joint_trajectory/status, /move_base/cancel] Move succeeded!
1633113001.156657934 INFO /stretch_driver [joint_trajectory_server.py:67(JointTrajectoryAction.execute_cb)] [topics: /stretch_controller/follow_joint_trajectory/feedback, /stretch/joint_states, /stretch_controller/follow_joint_trajectory/result, /stretch_controller/follow_joint_trajectory/status, /rosout, /magnetometer_mobile_base, /stretch_controller/follow_joint_trajectory/goal, /odom, /imu_wrist, /imu_mobile_base, /stretch_controller/follow_joint_trajectory/cancel, /stretch/cmd_vel] /stretch_driver joint_traj action: New trajectory received with joint_names = ['joint_head_tilt', 'joint_head_pan']
1633113001.171288967 INFO /stretch_driver [joint_trajectory_server.py:208(JointTrajectoryAction.success_callback)] [topics: /stretch_controller/follow_joint_trajectory/feedback, /stretch/joint_states, /stretch_controller/follow_joint_trajectory/result, /stretch_controller/follow_joint_trajectory/status, /rosout, /magnetometer_mobile_base, /stretch_controller/follow_joint_trajectory/goal, /odom, /imu_wrist, /imu_mobile_base, /stretch_controller/follow_joint_trajectory/cancel, /stretch/cmd_vel] /stretch_driver joint_traj action: Achieved all target points.
1633113001.905045986 INFO /funmap [navigate.py:297(MoveBase.forward)] [topics: /funmap/point_cloud2, /stretch/joint_states, /stretch_controller/follow_joint_trajectory/result, /move_base/feedback, /rosout, /tf, /clicked_point, /move_base/goal, /initialpose, /funmap/obstacle_point_cloud2, /stretch_controller/follow_joint_trajectory/feedback, /move_base/status, /move_base_simple/goal, /stretch_controller/follow_joint_trajectory/goal, /stretch_controller/follow_joint_trajectory/cancel, /funmap/voi_marker, /funmap/marker_array, /tf_static, /move_base/result, /camera/depth/color/points, /funmap/navigation_plan_markers, /stretch_controller/follow_joint_trajectory/status, /move_base/cancel] Obstacle detected near the front of the robot, so not starting to move forward.
1633113001.911160945 INFO /funmap [funmap:832(FunmapNode.navigate_to_map_pixel)] [topics: /funmap/point_cloud2, /stretch/joint_states, /stretch_controller/follow_joint_trajectory/result, /move_base/feedback, /rosout, /tf, /clicked_point, /move_base/goal, /initialpose, /funmap/obstacle_point_cloud2, /stretch_controller/follow_joint_trajectory/feedback, /move_base/status, /move_base_simple/goal, /stretch_controller/follow_joint_trajectory/goal, /stretch_controller/follow_joint_trajectory/cancel, /funmap/voi_marker, /funmap/marker_array, /tf_static, /move_base/result, /camera/depth/color/points, /funmap/navigation_plan_markers, /stretch_controller/follow_joint_trajectory/status, /move_base/cancel] Failed to reach forward motion goal.
1633113001.913170099 INFO /keyboard_teleop [keyboard_teleop:128(GetKeyboardCommands.get_command)] [topics: /stretch_controller/follow_joint_trajectory/feedback, /stretch/joint_states, /stretch_controller/follow_joint_trajectory/goal, /stretch_controller/follow_joint_trajectory/cancel, /keyboard_teleop/point_cloud2, /rosout, /tf, /tf_static, /stretch_controller/follow_joint_trajectory/result, /camera/depth/color/points, /stretch_controller/follow_joint_trajectory/status] trigger_result = success: False
message: "**Failed to reach forward motion goal.**"
1633113001.914146900 INFO /keyboard_teleop [keyboard_teleop:384(KeyboardTeleopNode.main)] [topics: /stretch_controller/follow_joint_trajectory/feedback, /stretch/joint_states, /stretch_controller/follow_joint_trajectory/goal, /stretch_controller/follow_joint_trajectory/cancel, /keyboard_teleop/point_cloud2, /rosout, /tf, /tf_static, /stretch_controller/follow_joint_trajectory/result, /camera/depth/color/points, /stretch_controller/follow_joint_trajectory/status] None
1633113002.660460923 WARN /ekf_se [/tmp/binarydeb/ros-melodic-robot-localization-2.6.7/src/ros_filter_utilities.cpp:127(RosFilterUtilities::lookupTransformSafe)] [topics: /rosout, /diagnostics, /tf, /odometry/filtered] Transform from imu_mobile_base to base_link was unavailable for the time requested. Using latest instead.
I GOT THE FOLLOWING ERROR when I tried using publish point to make the robot move to a 3D goal.
1633113404.248675107 ERROR /funmap [funmap:617(FunmapNode.plan_to_reach)] [topics: /funmap/point_cloud2, /stretch/joint_states, /stretch_controller/follow_joint_trajectory/result, /move_base/feedback, /rosout, /tf, /clicked_point, /move_base/goal, /initialpose, /funmap/obstacle_point_cloud2, /stretch_controller/follow_joint_trajectory/feedback, /move_base/status, /move_base_simple/goal, /stretch_controller/follow_joint_trajectory/goal, /stretch_controller/follow_joint_trajectory/cancel, /funmap/voi_marker, /funmap/marker_array, /tf_static, /move_base/result, /camera/depth/color/points, /funmap/navigation_plan_markers, /stretch_controller/follow_joint_trajectory/status, /move_base/cancel] **No valid base pose found for reaching the target.**
1633113404.250273942 ERROR /funmap [topics.py:753(_SubscriberImpl._invoke_callback)] [topics: /funmap/point_cloud2, /stretch/joint_states, /stretch_controller/follow_joint_trajectory/result, /move_base/feedback, /rosout, /tf, /clicked_point, /move_base/goal, /initialpose, /funmap/obstacle_point_cloud2, /stretch_controller/follow_joint_trajectory/feedback, /move_base/status, /move_base_simple/goal, /stretch_controller/follow_joint_trajectory/goal, /stretch_controller/follow_joint_trajectory/cancel, /funmap/voi_marker, /funmap/marker_array, /tf_static, /move_base/result, /camera/depth/color/points, /funmap/navigation_plan_markers, /stretch_controller/follow_joint_trajectory/status, /move_base/cancel] bad callback: <bound method FunmapNode.reach_to_click_callback of <__main__.FunmapNode instance at 0x7f03b836cd20>>
Traceback (most recent call last):
File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/topics.py", line 750, in _invoke_callback
cb(msg)
File "/home/aswingururaj/catkin_ws/src/stretch_ros/stretch_funmap/nodes/funmap", line 692, in reach_to_click_callback
success, message = self.navigate_to_map_pixel(robot_reach_xya_pix[:2],
TypeError: 'NoneType' object has no attribute '__getitem__'
Would you please let me know how I can go about solving them?
Thanks