Package: trajcontrol (version: trajcontrol_jhu)

Overview

This repository contains:

• ROS2 trajcontrol package (see nodes and message exchanges in Communication Diagram)
• Launch files for different control tasks for Lisa robot (see details in Usage)

Description

Subscribers

/needle/state/current_shape: "geometry_msgs/msg/PoseArray" - the pose array of the needle shape in 0.5 mm increments

stage/state/pose: "geometry_msgs/msg/PoseStamped - pose of the robot

Publishers

/stage/state/needle_pose: "geometry_msgs/msg/PoseStamped - current absolute insertion depth (x=0.0, y=abs(depth), z=0.0, q=[1,0,0,0])

Action client

/move_stage: "stage_control_interfaces/action/MoveStage" - action for moving the robot to given horizontal (x) and vertical (z) positions

Usage

Create a workspace and to the src folder, commit the following repositories:

• trajcontrol
• ros2_needle_guide_robot
• ros2_needle_shape_publisher
• ros2_hyperion_interrogator
• ros2_igtl_bridge

Remember to install OpenIGTLink

To build system packages:

  colcon build --cmake-args -DOpenIGTLink_DIR:PATH=<insert_path_to_openigtlink>/OpenIGTLink-build --symlink-install

To run in debug mode, include:

  --ros-args --log-level debug

Demo:

The trajcontrol demo emulates the needle insertion and robot motion using the keyboard. Open 2 terminals:

1. Launch trajcontrol demo:

  ros2 run trajcontrol trajcontrol_demo

2. Run keyboard node:

  ros2 run trajcontrol keypress

and use the keyboard to signal an insertion step. SPACE key emulates a 5mm insertion step without robot motion, while arrows from the numeric keyboard (2,4,6,8) emulate a 5mm insertion step with 1mm displacement of the robot in the corresponding up-down/left-right directions.

Registration procedure:

To be defined

Jacobian experimental initialization:

To be defined

Manually move the robot:

You may want to manually position the robot (in horizontal and vertical directions) using the keyboard.

To use robot in manual mode, open 3 terminals:

1. Launch PlusServer with configFile 'PlusDeviceSet_Server_NDIAurora_1Needle.xml'

  sudo /opt/PlusBuild-bin/bin/./PlusServerLauncher

2. Launch robot in manual:

  ros2 launch trajcontrol manual.launch.py

3. Run keyboard node:

  ros2 run trajcontrol keypress

and use arrows from the numeric keyboard (2,4,6,8) to move robot up-down/left-right No experimental data is recorded.

Move the robot to predefined positions:

You may need to move the robot to a pre-defined sequence of positions (waits 3.0s at each position before automatically moving to the next one)

To use robot in sequence mode, open 2 terminals:

1. Launch PlusServer with configFile 'PlusDeviceSet_Server_NDIAurora_1Needle.xml'

  sudo /opt/PlusBuild-bin/bin/./PlusServerLauncher

2. Launch robot in manual:

  ros2 launch trajcontrol sequence.launch.py filename:=NAME

Defining filename (default=my_data) is optional. The file defined by 'filename' is a csv with all experimental data and is it saved as 'data/NAME.csv'

Move robot to a fixed horizontal position:

You may need to position the robot at a pre-defined X (horizontal) position with Z (vertical) in manual mode. This is useful to perform the insertions at the same position with respect to the Aurora (and avoid parts of the measuring volume that are problematic).

To move robot to a fixed X, open 3 terminals:

1. Launch PlusServer with configFile 'PlusDeviceSet_Server_NDIAurora_1Needle.xml'

  sudo /opt/PlusBuild-bin/bin/./PlusServerLauncher

2. Launch robot in manual:

  ros2 launch trajcontrol init.launch.py

3. Run keyboard node:

  ros2 run trajcontrol keypress

and use arrows from the numeric keyboard (2,8) to move robot up-down No experimental data is recorded.

Control the robot using the data-driven MPC lateral compensation:

To run the trajectory control with MPC, open 3 terminals:

1. Launch PlusServer with configFile 'PlusDeviceSet_Server_NDIAurora_1Needle.xml'

  sudo /opt/PlusBuild-bin/bin/./PlusServerLauncher

2. Launch robot in manual:

  ros2 launch trajcontrol mpc_step.launch.py H:=4 filename:=NAME 

Defining H (default=5) and filename (default=my_data) are optional.

3. Run keyboard node:

  ros2 run trajcontrol keypress

and use SPACE key from the keyboard to signal each insertion step. Defining H(default:=5) and filename (default=my_data) are optional. The file defined by 'filename' is a csv with all experimental data and is it saved as 'data/NAME.csv'

Communication diagram

For 2023 demo:

Launch trajectory control package (robot, needle, sensor_processing, save_file, estimator and controller nodes)

ros2 launch trajcontrol mcp_step.launch.py filename:=name_for_data_file

Run keypress node in a different terminal

ros2 run trajcontrol keypress

For March 2022 demo:

Launch trajectory control package (savefile, estimator and controller nodes)

ros2 launch trajcontrol real_test.launch.py filename:=name_for_data_file

Run keypress node in a different terminal

ros2 run trajcontrol keypress