Skip to content

Latest commit

 

History

History
71 lines (48 loc) · 3.14 KB

README.md

File metadata and controls

71 lines (48 loc) · 3.14 KB

This package shows how 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
  2. 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 should be installed in your Python environment.

Open a terminal and clone the invariants_py_ros repository into your Catkin workspace:

cd ~/catkin_ws/src
git clone https://gitlab.kuleuven.be/robotgenskill/ros-packages/invariants_py_ros

Build the package and source the workspace:

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:

roslaunch invariants_py_ros launch_invars_calc.launch

Example

To test the calculation, you can use

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).

screenshot rviz

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