diff --git a/.gitmodules b/.gitmodules
index e27d30d..7456888 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -43,3 +43,12 @@
[submodule "third-party/usb_cam"]
path = third-party/usb_cam
url = git@github.com:ros-drivers/usb_cam.git
+[submodule "third-party/stomp_ros"]
+ path = third-party/stomp_ros
+ url = https://github.com/ros-industrial/stomp_ros.git
+[submodule "third-party/stomp"]
+ path = third-party/stomp
+ url = https://github.com/ros-industrial/stomp.git
+[submodule "third-party/ros_industrial_cmake_boilerplate"]
+ path = third-party/ros_industrial_cmake_boilerplate
+ url = https://github.com/ros-industrial/ros_industrial_cmake_boilerplate.git
diff --git a/aurmr_tasks/scripts/aurmr_demo b/aurmr_tasks/scripts/aurmr_demo
index 0aeceb0..7d3089f 100755
--- a/aurmr_tasks/scripts/aurmr_demo
+++ b/aurmr_tasks/scripts/aurmr_demo
@@ -23,10 +23,28 @@ from aurmr_perception.util import vec_to_quat_msg, I_QUAT
from aurmr_tasks.common.states import Wait
from aurmr_tasks.common.hri import UserPromptForRetry
+from aurmr_tasks.common import compliance_controller
from queue import Empty, Queue
-
-
+class PublishStatus(State):
+ def __init__(self, publisher, wait_time=0):
+ State.__init__(self, outcomes=['succeeded'], input_keys=["status", "start_time", "target_object_id"])
+ self.publisher=publisher
+ self.wait_time = wait_time
+ def execute(self, userdata):
+ now = rospy.get_rostime()
+ now = now.secs + now.nsecs*10**(-9)
+ time_delta = now - userdata["start_time"]
+
+
+ msg = PickStatus()
+ msg.status = userdata["status"]
+ msg.object_id = userdata["target_object_id"]
+ msg.time = time_delta
+ # self.publisher.publish("test")
+ self.publisher.publish(msg)
+ rospy.sleep(self.wait_time)
+ return "succeeded"
class PickStowQueue:
def __init__(self, pick_sm) -> None:
@@ -135,19 +153,33 @@ def main():
# These are pre-terminating states that note what went wrong in the userdata. This avoids
# encoding the same information in an explosion of named state transitions
cf.splat_auto("SPLAT_PICK", "request", ["target_bin_id", "target_object_id", "target_object_asin"])
- # StateMachine.add_auto("ADD_IN_HAND_COLLISION_BOX_PRE_GRASP", planning_scene.AddInHandCollisionGeometry(robot), ["succeeded", "aborted"])
- StateMachine.add("GET_GRASP_POSE", perception.GetGraspPose(robot.tf2_buffer, pre_grasp_offset=.015),
- {"succeeded": "REMAP_PRE_GRASP_POSE", "aborted":"NOTE_PASS", "preempted":"NOTE_PASS"})
+ # StateMachine.add_auto("ADD_IN_HAND_COLLISION_BOX_PRE_GRASP", motion.AddInHandCollisionGeometry(robot), ["succeeded", "aborted"])
+
+
+ StateMachine.add_auto("CAPTURE_POINTS", perception.GetPoints(), ["succeeded", "aborted"])
+
+ StateMachine.add("GET_GRASP_POSE", perception.GetGraspPose(robot.tf2_buffer, pre_grasp_offset=.065),
+ {"succeeded": "MOVE_TO_BIN", "aborted":"NOTE_PASS", "preempted":"NOTE_PASS"})
+
+ #StateMachine.add("PASS", PublishStatus(status_pub, wait_time=3), {"succeeded": "done"})
+
+ #StateMachine.add("GET_GRASP_POSE", perception.GetGraspPose(robot.tf2_buffer, pre_grasp_offset=.035),
+ # {"succeeded": "REMAP_PRE_GRASP_POSE", "aborted":"NOTE_PASS", "preempted":"NOTE_PASS"})
cf.inject_userdata("NOTE_PASS", "done", "status", PickRequestResponse.OUTCOME_PERCEPTION_FAILURE)
+ StateMachine.add_auto("MOVE_TO_BIN", motion.MoveToBin(robot), ["succeeded", "aborted"])
+
+ #StateMachine.add_auto("PUBLISH_CURRENTLY_PICKING", PublishStatus(status_pub), ["succeeded"])
+ # StateMachine.add_auto("ADD_IN_HAND_COLLISION_BOX_PRE_GRASP", planning_scene.AddInHandCollisionGeometry(robot), ["succeeded", "aborted"])
+
StateMachine.add_auto("REMAP_PRE_GRASP_POSE", cf.input_to_output("pre_grasp_pose", "pose"), ["succeeded"])
StateMachine.add_auto("CLEAR_SCENE_PREMOVE", planning_scene.ClearCollisionGeometry(robot), ["succeeded", "aborted"])
StateMachine.add_auto("SETUP_COLLISION_SCENE_PREMOVE", planning_scene.AddFullPodCollisionGeometry(robot), ["succeeded", "aborted"])
- StateMachine.add("MOVE_TO_PRE_GRASP", motion.MoveEndEffectorToPoseManipulable(robot), {"succeeded": "CLEAR_SCENE_PREGRASP", "preempted": "NOTE_PREEMPTION", "aborted": "NOTE_PLANNING_FAILURE"})
- cf.inject_userdata("NOTE_PLANNING_FAILURE", "done", "status", PickRequestResponse.OUTCOME_PLANNING_FAILURE)
- StateMachine.add_auto("CLEAR_SCENE_PREGRASP", planning_scene.ClearCollisionGeometry(robot), ["succeeded", "aborted"])
- StateMachine.add_auto("SETUP_COLLISION_SCENE_PREGRASP", planning_scene.AddPartialPodCollisionGeometry(robot), ["succeeded", "aborted"])
+ # FIXME: if `MOVE_TO_PREBIN`` returns early, IMPLEMENT a method to check that move group has reached goal
+ StateMachine.add_auto("MOVE_TO_PRE_GRASP", motion.MoveEndEffectorToPose(robot), ["succeeded", "aborted", "preempted"])
+ StateMachine.add_auto("CLEAR_SCENE_PREGRASP", motion.ClearCollisionGeometry(robot), ["succeeded", "aborted"])
+ StateMachine.add_auto("SETUP_COLLISION_SCENE_PREGRASP", motion.AddPartialPodCollisionGeometry(robot), ["succeeded", "aborted"])
StateMachine.add_auto("MOVE_TO_GRASP", motion.grasp_move_to_offset(robot, (0, 0, 0.5)), ["succeeded", "aborted", "preempted"])
StateMachine.add_auto("LIFT_OBJ", motion.robust_move_to_offset(robot, (0, 0, 0.035), 'base_link'), ["succeeded", "aborted", "preempted"])
StateMachine.add_auto("ADJUST_RIGHT_IF_COLUMN_1", motion.AdjustRightIfColumn1(robot, (0, 0.05, 0)), ["succeeded", "aborted", "pass"])
@@ -176,7 +208,7 @@ def main():
with pick_retry_sm:
StateMachine.add_auto("BLOW_OFF_GRIPPER", motion.BlowOffGripper(robot, return_before_done=True), ["succeeded", "aborted", "preempted"])
- StateMachine.add_auto("UNBLOCK_VIEW", motion.MoveToJointAngles(robot, "hide"), ["succeeded", "aborted"])
+ StateMachine.add_auto("UNBLOCK_VIEW", motion.MoveToJointAngles(robot, "drop_hide"), ["succeeded", "aborted"])
StateMachine.add_auto("PICTURE_WAIT", Wait(2), ["succeeded"])
StateMachine.add_auto("UPDATE_BIN_MASKS", perception.UpdateBin(), ["succeeded", "preempted", "aborted"])
StateMachine.add('USER_PROMPT_FOR_RETRY', UserPromptForRetry(rospy.get_param("bin_bounds"), robot.tf2_buffer, timeout_connection_secs=1.0), {"retry": "REMAP_GRASP", "continue": "continue"})
diff --git a/aurmr_tasks/src/aurmr_tasks/common/compliance_controller.py b/aurmr_tasks/src/aurmr_tasks/common/compliance_controller.py
index ccc1950..ea353d4 100644
--- a/aurmr_tasks/src/aurmr_tasks/common/compliance_controller.py
+++ b/aurmr_tasks/src/aurmr_tasks/common/compliance_controller.py
@@ -4,11 +4,14 @@
from geometry_msgs.msg import PoseStamped
import time
+import numpy as np
+from aurmr_tasks.util import pose_dist
+
from aurmr_tasks.common.visualization_utils import EEFVisualization
class MoveToOffset(State):
- def __init__(self, robot, offset, frame_id, detect_object,idle_timeout=3):
- super().__init__(outcomes=['succeeded', 'aborted'], output_keys=['starting_grasp_pose_out'], input_keys=['target_pose_in'])
+ def __init__(self, robot, offset, frame_id, detect_object, idle_timeout=3):
+ super().__init__(outcomes=['succeeded', 'aborted'], input_keys=['offset_moved_in'], output_keys=['offset_moved_out'])
self.robot = robot
self.offset = offset
self.frame_id = frame_id
@@ -28,30 +31,36 @@ def stop(self):
def execute(self, userdata):
self.close_robot_gripper()
- start_pose = self.robot.move_group.get_current_pose()
- start_pose.header.stamp = rospy.Time(0)
- start_pose = self.robot.tf2_buffer.transform(start_pose, self.target_frame, rospy.Duration(1))
-
- userdata.starting_grasp_pose_out = start_pose
- rospy.logdebug("setting starting pose to ", start_pose)
- self.vis_out.visualize_eef(start_pose)
+ start_pose = self.current_pose_in_target_frame()
- if userdata.target_pose_in:
- target_pose = userdata.target_pose_in
+ if self.offset is None:
+ offset = userdata.offset_moved_in
else:
- target_pose = self.calculate_target_pose(start_pose)
- self.robot.force_values_init = True
+ offset = self.offset
+
+ target_pose = self.calculate_target_pose(start_pose, offset)
+ self.robot.force_values_init = True
self.vis.visualize_eef(target_pose)
self.publish_target_pose(target_pose)
- return self.wait_until_stopped(0.001, self.idle_timeout)
+ result = self.wait_until_stopped(0.001, self.idle_timeout)
+
+ current_pose = self.current_pose_in_target_frame()
+ current_pose = current_pose.pose
+ start_pose = start_pose.pose
+ offset_moved = np.array([start_pose.position.x, start_pose.position.y, start_pose.position.z])
+ offset_moved = offset_moved - np.array([current_pose.position.x, current_pose.position.y, current_pose.position.z])
+
+ userdata.offset_moved_out = offset_moved
+
+ return result
def close_robot_gripper(self):
self.robot.close_gripper(return_before_done=True)
- def calculate_target_pose(self, start_pose):
- target_pose = apply_offset_to_pose(start_pose, self.offset, self.frame_id, self.robot.tf2_buffer)
+ def calculate_target_pose(self, start_pose, offset):
+ target_pose = apply_offset_to_pose(start_pose, offset, self.frame_id, self.robot.tf2_buffer)
return self.robot.tf2_buffer.transform(target_pose, self.target_frame, rospy.Duration(1))
def publish_target_pose(self, target_pose):
@@ -99,7 +108,6 @@ def current_pose_in_target_frame(self):
return current_pose
-
def check_object_condition(self):
return (self.detect_object and self.robot.check_gripper_item())
diff --git a/aurmr_tasks/src/aurmr_tasks/common/motion.py b/aurmr_tasks/src/aurmr_tasks/common/motion.py
index 1e87363..e84a837 100644
--- a/aurmr_tasks/src/aurmr_tasks/common/motion.py
+++ b/aurmr_tasks/src/aurmr_tasks/common/motion.py
@@ -85,13 +85,13 @@ def execute(self, userdata):
pose = userdata["pose"]
success = self.robot.move_to_pose(
pose,
- allowed_planning_time=10.0,
+ allowed_planning_time=20.0,
execution_timeout=15.0,
- num_planning_attempts=12,
+ num_planning_attempts=40,
orientation_constraint=None,
replan=True,
- replan_attempts=8,
- tolerance=0.01)
+ replan_attempts=20,
+ tolerance=0.02)
if success:
return "succeeded"
else:
@@ -299,6 +299,24 @@ def execute(self, userdata):
else:
return "succeeded"
+class MoveToBin(State):
+ def __init__(self, robot):
+ State.__init__(self, input_keys=["target_bin_id"], outcomes=['succeeded', 'aborted'])
+ self.robot = robot
+
+ def execute(self, ud):
+ target = ud["target_bin_id"]
+ if isinstance(target, JointState):
+ to_log = target.position
+ else:
+ target = "pre_bin_" + target.lower()
+ to_log = target
+ rospy.loginfo(f"Moving to {to_log}")
+ success = self.robot.move_to_joint_angles(target, return_before_done=False)
+ if success:
+ return "succeeded"
+ else:
+ return "aborted"
class CloseGripper(State):
def __init__(self, robot, return_before_done=False):
@@ -350,4 +368,3 @@ def execute(self, ud):
if not self.robot.check_gripper_item():
self.robot.open_gripper(return_before_done=True)
return 'succeeded'
-
diff --git a/aurmr_tasks/src/aurmr_tasks/common/perception.py b/aurmr_tasks/src/aurmr_tasks/common/perception.py
index acb952e..5e51b92 100644
--- a/aurmr_tasks/src/aurmr_tasks/common/perception.py
+++ b/aurmr_tasks/src/aurmr_tasks/common/perception.py
@@ -150,18 +150,47 @@ def execute(self, userdata):
return "aborted"
+class GetPoints(State):
+ def __init__(self, frame_id='base_link'):
+ State.__init__(
+ self,
+ input_keys=['target_bin_id', 'target_object_id'],
+ output_keys=['points_response'],
+ outcomes=['succeeded', 'aborted']
+ )
+ self.get_points = rospy.ServiceProxy('/aurmr_perception/get_object_points', GetObjectPoints)
+ self.get_points.wait_for_service(timeout=5)
+ self.frame_id = frame_id
+
+ def execute(self, userdata):
+ try:
+ get_points_req = GetObjectPointsRequest(
+ bin_id=userdata['target_bin_id'],
+ object_id=userdata['target_object_id'],
+ frame_id=self.frame_id
+ )
+ print(get_points_req, get_points_req)
+ points_response = self.get_points(get_points_req)
+
+ if not points_response.success:
+ userdata["status"] = "pass"
+ return "aborted"
+
+ userdata['points_response'] = points_response
+ return "succeeded"
+ except:
+ return "aborted"
+
class GetGraspPose(State):
def __init__(self, tf_buffer, frame_id='base_link', pre_grasp_offset=.12):
State.__init__(
self,
- input_keys=['target_bin_id', 'target_object_id'],
+ input_keys=['target_bin_id', 'target_object_id', 'points_response'],
output_keys=['grasp_pose', 'pre_grasp_pose'],
outcomes=['succeeded', 'preempted', 'aborted']
)
- self.get_points = rospy.ServiceProxy('/aurmr_perception/get_object_points', GetObjectPoints)
self.get_grasp = rospy.ServiceProxy('/grasp_detection/detect_grasps', DetectGraspPoses)
# Crash during initialization if these aren't running so see the problem early
- self.get_points.wait_for_service(timeout=5)
self.get_grasp.wait_for_service(timeout=5)
self.frame_id = frame_id
self.pre_grasp_offset = pre_grasp_offset
@@ -180,34 +209,41 @@ def execute(self, userdata):
bin_id=userdata['target_bin_id'],
object_id=userdata['target_object_id'],
frame_id=self.frame_id
+
)
- points_response = self.get_points(get_points_req)
- if not points_response.success:
- return "aborted"
+ try:
- grasp_response = self.get_grasp(points=points_response.points,
- mask=points_response.mask,
- dist_threshold=self.pre_grasp_offset, bin_id=userdata['target_bin_id'])
+ points_response = userdata["points_response"]
- if not grasp_response.success:
- return "aborted"
+ if not points_response.success:
+ return "aborted"
+
+
+ grasp_response = self.get_grasp(points=points_response.points,
+ mask=points_response.mask,
+ dist_threshold=self.pre_grasp_offset, bin_id=userdata['target_bin_id'])
- # NOTE: No extra filtering or ranking on our part. Just take the first one
- # As the arm_tool0 is 26cm in length w.r.t tip of suction cup thus adding 0.26m offset
- # TODO(henrifung): Pull this number from xacro, perhaps from userdata
- grasp_pose = grasp_response.poses[0]
+ if not grasp_response.success:
+ return "aborted"
- grasp_pose = add_offset(-0.26, grasp_pose)
+ # NOTE: No extra filtering or ranking on our part. Just take the first one
+ # As the arm_tool0 is 26cm in length w.r.t tip of suction cup thus adding 0.26m offset
+ # TODO(henrifung): Pull this number from xacro, perhaps from userdata
+ grasp_pose = grasp_response.poses[0]
- userdata['grasp_pose'] = grasp_pose
+ grasp_pose = add_offset(-0.26, grasp_pose)
- # adding 0.12m offset for pre grasp pose to prepare it for grasp pose which is use to pick the object
- pregrasp_pose = add_offset(-self.pre_grasp_offset, grasp_pose)
+ userdata['grasp_pose'] = grasp_pose
- userdata['pre_grasp_pose'] = pregrasp_pose
+ # adding 0.12m offset for pre grasp pose to prepare it for grasp pose which is use to pick the object
+ pregrasp_pose = add_offset(-self.pre_grasp_offset, grasp_pose)
- self.pose_viz.publish(grasp_pose)
- self.pre_grasp_viz.publish(pregrasp_pose)
+ userdata['pre_grasp_pose'] = pregrasp_pose
- return "succeeded"
+ self.pose_viz.publish(grasp_pose)
+ self.pre_grasp_viz.publish(pregrasp_pose)
+
+ return "succeeded"
+ except:
+ return "aborted"
diff --git a/aurmr_tasks/src/aurmr_tasks/common/tahoma.py b/aurmr_tasks/src/aurmr_tasks/common/tahoma.py
index f694d94..077b7e5 100644
--- a/aurmr_tasks/src/aurmr_tasks/common/tahoma.py
+++ b/aurmr_tasks/src/aurmr_tasks/common/tahoma.py
@@ -361,8 +361,9 @@ def move_to_joint_angles(self,
plan_only=False,
replan=False,
replan_attempts=5,
- tolerance=0.01,
- path_constraints=None):
+ tolerance=0.005,
+ path_constraints=None,
+ return_before_done=False):
"""Moves the end-effector to a pose, using motion planning.
Args:
@@ -407,7 +408,7 @@ def move_to_joint_angles(self,
self.move_group.set_named_target(joints)
else:
self.move_group.set_joint_value_target(joints)
- self.move_group.go(wait=True)
+ self.move_group.go(wait=not return_before_done)
# Calling ``stop()`` ensures that there is no residual movement
self.move_group.stop()
@@ -941,3 +942,8 @@ def compute_ik(self, pose_stamped, timeout=rospy.Duration(5)):
def cancel_all_goals(self):
self._move_group_client.cancel_all_goals()
self._joint_traj_client.cancel_all_goals()
+
+ def reached_goal(self):
+ while self._move_group_client.get_state() == GoalStatus.ACTIVE:
+ continue
+ return True
\ No newline at end of file
diff --git a/tahoma_bringup/config/controllers.yaml b/tahoma_bringup/config/controllers.yaml
index ab98dcb..0a1b14f 100644
--- a/tahoma_bringup/config/controllers.yaml
+++ b/tahoma_bringup/config/controllers.yaml
@@ -221,11 +221,11 @@ ur_cartesian_compliance_controller:
rot_x: 5.0
rot_y: 5.0
rot_z: 5.0
- max_translation_error_norm: 25
- max_rotation_error_norm: 5.14
+ max_translation_error_norm: 0.2
+ max_rotation_error_norm: 0.1
solver:
- error_scale: 0.35
+ error_scale: 0.5
iterations: 1
publish_state_feedback: True
diff --git a/tahoma_moveit_config/config/stomp_planning.yaml b/tahoma_moveit_config/config/stomp_planning.yaml
new file mode 100644
index 0000000..97192c5
--- /dev/null
+++ b/tahoma_moveit_config/config/stomp_planning.yaml
@@ -0,0 +1,37 @@
+optimization:
+ num_timesteps: 60
+ num_iterations: 40
+ num_iterations_after_valid: 0
+ num_rollouts: 30
+ max_rollouts: 30
+ initialization_method: 1 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST]
+ control_cost_weight: 0.0
+task:
+ noise_generator:
+ - class: stomp_moveit/NormalDistributionSampling
+ stddev: [0.05, 0.8, 1.0, 0.8, 0.4, 0.4]
+ cost_functions:
+ - class: stomp_moveit/CollisionCheck
+ collision_penalty: 1.0
+ cost_weight: 1.0
+ kernel_window_percentage: 0.2
+ longest_valid_joint_move: 0.05
+ noisy_filters:
+ - class: stomp_moveit/JointLimits
+ lock_start: True
+ lock_goal: True
+ - class: stomp_moveit/MultiTrajectoryVisualization
+ line_width: 0.02
+ rgb: [255, 255, 0]
+ marker_array_topic: stomp_trajectories
+ marker_namespace: noisy
+ update_filters:
+ - class: stomp_moveit/PolynomialSmoother
+ poly_order: 6
+ - class: stomp_moveit/TrajectoryVisualization
+ line_width: 0.05
+ rgb: [0, 191, 255]
+ error_rgb: [255, 0, 0]
+ publish_intermediate: True
+ marker_topic: stomp_trajectory
+ marker_namespace: optimized
diff --git a/tahoma_moveit_config/config/tahoma.srdf b/tahoma_moveit_config/config/tahoma.srdf
index b5bd223..3110507 100644
--- a/tahoma_moveit_config/config/tahoma.srdf
+++ b/tahoma_moveit_config/config/tahoma.srdf
@@ -48,30 +48,190 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/tahoma_moveit_config/launch/demo.launch b/tahoma_moveit_config/launch/demo.launch
index 8db5e25..d66b770 100644
--- a/tahoma_moveit_config/launch/demo.launch
+++ b/tahoma_moveit_config/launch/demo.launch
@@ -1,7 +1,7 @@
-
+
diff --git a/tahoma_moveit_config/launch/move_group.launch b/tahoma_moveit_config/launch/move_group.launch
index ae38de5..906a8da 100644
--- a/tahoma_moveit_config/launch/move_group.launch
+++ b/tahoma_moveit_config/launch/move_group.launch
@@ -12,7 +12,7 @@
-
+
diff --git a/tahoma_moveit_config/launch/stomp_planning_pipeline.launch.xml b/tahoma_moveit_config/launch/stomp_planning_pipeline.launch.xml
new file mode 100644
index 0000000..3beccab
--- /dev/null
+++ b/tahoma_moveit_config/launch/stomp_planning_pipeline.launch.xml
@@ -0,0 +1,33 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/third-party/ros_industrial_cmake_boilerplate b/third-party/ros_industrial_cmake_boilerplate
new file mode 160000
index 0000000..559e91b
--- /dev/null
+++ b/third-party/ros_industrial_cmake_boilerplate
@@ -0,0 +1 @@
+Subproject commit 559e91b734f3e23131567404be323926883dcce7
diff --git a/third-party/stomp b/third-party/stomp
new file mode 160000
index 0000000..f9e9d78
--- /dev/null
+++ b/third-party/stomp
@@ -0,0 +1 @@
+Subproject commit f9e9d78f741f0779b65333d4f54d68ade740520b
diff --git a/third-party/stomp_ros b/third-party/stomp_ros
new file mode 160000
index 0000000..882784c
--- /dev/null
+++ b/third-party/stomp_ros
@@ -0,0 +1 @@
+Subproject commit 882784c6952e6456c85d0f61686913a33309bcfe