Calling write_state from a python or bash script is not working, I'm looking for a way to successfully call write_state.

edit

This is a first time inquiry on ROS answers, I've been learning about and working on ROS melodic since March; I've tried to make the inquiry as complete as possible. I'm on a team developing a handheld scanner for performing real-time SLAM along with other, non-ROS sensor fusion.

System Architecture: NVidia Jetson AGX Xavier connected to a Realsense T265 and Occipital Structure Core, developing in ROS Melodic

ROS environment variables: ROS_ETC_DIR=/opt/ros/melodic/etc/ros ROS_ROOT=/opt/ros/melodic/share/ros ROS_MASTER_URI=http://localhost:11311 ROS_VERSION=1 ROS_PYTHON_VERSION=2 ROS_PACKAGE_PATH=/media/prm-jetson/slam_sd/H3D_SLAM/h3dslammapping/src:/home/prm-jetson/slam_test/src:/opt/ros/melodic/share ROSLISP_PACKAGE_DIRECTORIES=/media/prm-jetson/slam_sd/H3D_SLAM/h3dslammapping/devel/share/common-lisp:/home/prm-jetson/slam_test/devel/share/common-lisp ROS_DISTRO=melodic

Details: I successfully collect a bag, finish trajectory, and write state from the command line; even though write_state seems only randomly successful. I'm working with a python script (pasted below) that calls the launch file for the mapping nodes, start and stop the bag collection, and launch rviz using rospy. I've been successful in finishing the trajectory using subprocess.call using a bash script; thus far, however, I am unable to serialize the data by calling write_state via rospy.ServiceProxy in the python script or in the bash script. I've had no luck troubleshooting why finish_trajectory works, but write_state doesn't. There is no output from running cartographer_rosbag_validate, so presumably the bagfile is fine. I pasted the codes below; since everything from the launch file executes, I did not attach it. I can attach the launch and lua files if provided enough credits to do so.

The python script:

import rospy, roslaunch, sys

from subprocess import call

from std_msgs.msg import String

from std_msgs.msg import Bool

from std_msgs.msg import Int32

from cartographer_ros_msgs.srv import FinishTrajectory

from cartographer_ros_msgs.srv import WriteState

pose_graph_filename = "filename: '$media/prm-jetson/slam_sd/Realsense_bags/h3d_test1_cameras_062920.bag.pbstream'"

cli_args = ['/media/prm-jetson/slam_sd/H3D_SLAM/h3dslammapping/src/sensor_ros/ROS/sc_cartographer/launch/rs_and_single_sc_driver.launch', '/media/prm-jetson/slam_sd/Realsense_bags/h3d_test1_cameras_062920.bag']

roslaunch_args = cli_args[0]

rospy.init_node('sensor_Mapping', anonymous=True)

uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)

roslaunch.configure_logging(uuid)

launch = roslaunch.scriptapi.ROSLaunch()

launch.parent = roslaunch.parent.ROSLaunchParent(uuid, [cli_args[0]])

launch.start()

rospy.loginfo("started")

rospy.sleep(15) # hold pause 15 seconds while bagfile is collected

then, either initiate subprocess.call OR the service proxies, written as a service client:

rospy.wait_for_service('finish_trajectory')

finish_trajectory = rospy.ServiceProxy('finish_trajectory', FinishTrajectory)

try: fin1 = finish_trajectory(0)

except rospy.ServiceException as exc: print("Service did not process request: " + str(exc))

rospy.sleep(5)

write_state = rospy.ServiceProxy('write_state', WriteState)

try: wrt1 = write_state('h3d_test1_cameras_062620.bag.pbstream',True) except rospy.ServiceException as exc: print("Service did not process request: " + str(exc))

rospy.sleep(20)

launch.stop()

The Bash Script As indicated above, finishing the trajectory works in the bash script, but write state does not. I initiate the bash script after the 15 second pause in the python script above with: call("/media/prm-jetson/slam_sd/H3D_SLAM/h3dslammapping/scripts/ws")

rosservice call ... (more)