Zero Joint Values in Initial Tesseract Environment State

[NithishkumarS]

Description

When initializing the Tesseract environment, the joint state values for the UR10e robot are consistently returning zero, regardless of the method used to retrieve them. This behavior is observed in two distinct approaches: directly from the environment and from the Tesseract environment monitor interface.

Environment

• ROS 2 Distribution: Jazzy
• Tesseract Version: 0.29.2
• Operating System: Ubuntu 24
• Hardware: UR10e

Expected Behavior

The initial joint values for the UR10e should be populated with the current values published on the /joint_states topic.

Actual Behavior

Both approaches result in an array of zero values for the UR10e joints.

Steps to Reproduce

Approach 1: Using ROSEnvironmentMonitor

1. Create and initialize a tesseract_environment::Environment object.
2. Instantiate a tesseract_monitoring::ROSEnvironmentMonitor and provide the current ROS node, the environment object, and a namespace.
3. Call monitor->startStateMonitor(joint_states_topic, false); to begin listening for joint state updates.
4. Attempt to retrieve the joint values from the environment.

Code Snippet:

env = std::make_shared<tesseract_environment::Environment>();
locator = std::make_shared<tesseract_rosutils::ROSResourceLocator>();

if (!env->init(urdf_xml_string, srdf_xml_string, locator)) {
    RCLCPP_ERROR(logger, "Could not initialise environment!");
    exit(1);
}

// Make a tesseract environment monitor
monitor = std::make_shared<tesseract_monitoring::ROSEnvironmentMonitor>(this->getNode(), env, MONITOR_NAMESPACE);
RCLCPP_ERROR(logger, "Joint states topic: %s", joint_states_topic.c_str());
monitor->startStateMonitor(joint_states_topic, false);
monitor->startPublishingEnvironment();

auto init_joint_values = this->env->getCurrentJointValues(joint_names);

Approach 2: Using ROSEnvironmentMonitorInterface

1. Initialize a tesseract_monitoring::ROSEnvironmentMonitorInterface.
2. Use the interface to get the environment state.
3. Call env_state.getJointValues(joint_names) to retrieve the joint values.

Code Snippet:

tesseract_monitoring::ROSEnvironmentMonitorInterface interface;

auto scene_state = interface.getEnvironmentState(MONITOR_NAMESPACE);
auto init_joint_values = scene_state.getJointValues(joint_names);

Additional Information

• Observation: When the robot is manually moved in the simulation to the zero joint state and once planning and execution succeeds, the issue disappears on the next run (Same nodes are still up). WaitForCurrentState(n); didn't make a difference either.

[marip8]

I think the problem might be that the node is not spun after the creation of the environment monitor, so it never has a chance to get joint values from the /joint_states topic.

Does something like this solve the problem?

// Make a tesseract environment monitor
monitor = std::make_shared<tesseract_monitoring::ROSEnvironmentMonitor>(this->getNode(), env, MONITOR_NAMESPACE);
RCLCPP_ERROR(logger, "Joint states topic: %s", joint_states_topic.c_str());
monitor->startStateMonitor(joint_states_topic, false);
monitor->startPublishingEnvironment();
+ ros::spinOnce();
auto init_joint_values = this->env->getCurrentJointValues(joint_names);

Otherwise, you could try to extract the joint values in a different function that gets called after the node starts spinning. Or try using an async spinner and give it a small amount of time after creation before trying to get the joint values