Does ROSEnvironmentMonitor publish Environment::setState() changes correctly?

[rjoomen]

Situation: I have a node owning an environment, with a ROSEnvironmentMonitor publishing it. There is also an rviz node running with a Tesseract Workbench subscribed to the tesseract_published_environment. If my node changes the joint states using Environment::setState(), the workbench does not reflect this change.

The topic /tesseract_published_environment correctly shows the updated joint states, but the time stamp of the joint_state header has not changed, so the monitoring environment does not pick up the change. This is caused by the timestamp of the joint states being set to last_robot_motion_time_, which is updated by a current_state_monitor, but only if it has subscribed to a joint_state topic.

Is this by design and should I update joint states in an environment in a different way, or should this be fixed?

(BTW: This situation can be easily reproduced by adding ros::WallDuration(10).sleep(); to the pick_and_place_example_node.cpp, just before line PickAndPlaceExample example(env, plotter);. This way the workbench is fully initialized before the example executes its setState() call and the example starts with the robot at all zeroes.)

[rjoomen]

@Levi-Armstrong?

[Levi-Armstrong]

I would say that was an oversight and should be fixed. I look into it further in about an hour.

[Levi-Armstrong]

It looks like the environment object does correctly update the state changed class member, so now looking at the environment monitor.

[Levi-Armstrong]

It looks like the environment monitor needs to be updated to handle a case where you are not monitoring a topic but changing the environment directly. I think the ideal option is in the case where you are not monitoring a joint state topic you add a callback to the environment object which may be able to call the same functions to update the internal state of the environment monitor. I believe this is an either or situation so you could not be monitoring a joint state topic and use setState.

[rjoomen]

Would this be an acceptable solution? (I'll also PR this to tesseract_ros when finished.)

[Levi-Armstrong]

Yea, the only thing I would change is to also update the last_update_time_ last_update_time_ = last_robot_motion_time_ = node_->now();

[rjoomen]

Done: tesseract-robotics/tesseract_ros2#45