https://github.com/trajectory-invariants/invariants_py_ros
Example of using Trajectory Invariants in ROS
https://github.com/trajectory-invariants/invariants_py_ros
Last synced: 26 days ago
JSON representation
Example of using Trajectory Invariants in ROS
- Host: GitHub
- URL: https://github.com/trajectory-invariants/invariants_py_ros
- Owner: trajectory-invariants
- License: mit
- Created: 2024-03-05T16:47:52.000Z (over 1 year ago)
- Default Branch: main
- Last Pushed: 2024-10-23T09:34:56.000Z (8 months ago)
- Last Synced: 2025-04-01T02:11:44.428Z (2 months ago)
- Language: Python
- Homepage: https://trajectory-invariants.github.io/
- Size: 12 MB
- Stars: 2
- Watchers: 1
- Forks: 0
- Open Issues: 0
-
Metadata Files:
- Readme: README.md
- License: LICENSE
Awesome Lists containing this project
README
This package shows how [invariants_py](https://github.com/trajectory-invariants/invariants_py) can be integrated in a ROS application.
Currently there are two examples:
1. online calculation of trajectory invariants from trajectory data on a subscribed ROS topic with message type `geometry_msgs/Pose`
1. online trajectory generation from an invariant model towards a changing position provided by a ROS topic with message type `geometry_msgs/Point`
### Installation
Dependencies:
- ROS Noetic on Ubuntu 20.04 is assumed to be installed. Other versions of ROS and Ubuntu should normally work as well but haven't been tested.
- A catkin workspace is assumed to be created (e.g. at `~/catkin_ws`) and configured for Python 3 using `catkin_make -DPYTHON_EXECUTABLE=/usr/bin/python3`.
- [invariants_py](https://github.com/trajectory-invariants/invariants_py) should be installed in your Python environment.
Open a terminal and clone the [invariants_py_ros](https://gitlab.kuleuven.be/robotgenskill/ros-packages/invariants_py_ros) repository into your Catkin workspace:
```shell
cd ~/catkin_ws/src
git clone https://gitlab.kuleuven.be/robotgenskill/ros-packages/invariants_py_ros
```
Build the package and source the workspace:
```shell
cd ~/catkin_ws/src
catkin_make
source devel/setup.bash
rospack profile
```
### Online invariants calculation
The _ros_invariants_calculation_ node calculates the vector invariants for a point trajectory.
Overview of topics:
- **/pose_data** (input): measured pose, message type: `geometry_msgs/Pose`
- **/pose_data_stamped** (output): measured pose with time-stamp to enable visualization, message type: `geometry_msgs/PoseStamped`
- **/trajectory_online** (output): estimated trajectory by the invariant optimization, message type: `nav_msgs.msg/Path`
The node can be launched using:
```shell
roslaunch invariants_py_ros launch_invars_calc.launch
```
#### Example
To test the calculation, you can use
```shell
roslaunch invariants_py_ros launch_invars_calc_test.launch
```
which uses prerecorded data from a bagfile and includes visualization in rviz.
Suggested improvements:
- Only the position data is being used currently. Extend the code so that invariants for rotation are included. To deal with pose messages check the etasl_invariants_integration repository in ERC/python_projects
- The invariants are now time-based using a window of measurements wrt time. How would you obtain time-invariance, a.k.a. geometric invariants that are defined w.r.t. a certain progress variable?
- Options to visualize results:
-- put results on ROS topics and make an additional ROS node that plots the data in Python
-- put results on ROS topics and use RQTgraph and/or rviz
Quick first test of vizualization in rviz (black = estimated trajectory of the measured object inside window of OCP, axes = current pose measurement but only the position is being used now).
The estimated trajectory is always trailing behind the measurement as expected. There is a small delay between measurement and window, to be checked (note that invariants component is limited to 20 Hz).
### Online trajectory generation
TODO