diff --git a/src/rove_bringup/launch/common.launch.py b/src/rove_bringup/launch/common.launch.py
index 500fe90..5cb260e 100644
--- a/src/rove_bringup/launch/common.launch.py
+++ b/src/rove_bringup/launch/common.launch.py
@@ -109,11 +109,11 @@ def generate_launch_description():
return LaunchDescription([
robot_state_publisher,
- robot_localization_node_local,
+ # robot_localization_node_local,
# robot_localization_node_global,
#navsat_transform,
twist_mux,
rviz,
teleop,
- autonomy,
+ # autonomy,
])
diff --git a/src/rove_bringup/launch/sim.launch.py b/src/rove_bringup/launch/sim.launch.py
index d06a1dc..29a3cd2 100644
--- a/src/rove_bringup/launch/sim.launch.py
+++ b/src/rove_bringup/launch/sim.launch.py
@@ -137,20 +137,6 @@ def generate_launch_description():
}.items(),
)
- static_tf_ovis = Node(
- package="tf2_ros",
- executable="static_transform_publisher",
- arguments=[
- "0.25",
- "0",
- "0.25",
- "0",
- "0",
- "3.14",
- "base_link",
- "ovis_base_link",
- ],
- )
ovis = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
@@ -159,10 +145,12 @@ def generate_launch_description():
launch_arguments={
"with_rove": "true",
"with_joy": "false",
- "ovis_base_origin": "0.25 0 0.25 0 0 3.14",
+ "ovis_base_origin": "0.22 0 0.4 0 0 3.14",
}.items(),
)
+
+
return LaunchDescription(
[
gz_sim,
@@ -174,6 +162,5 @@ def generate_launch_description():
common,
# human_tracker,
ovis,
- static_tf_ovis,
]
)
diff --git a/src/rove_description/urdf/gazebo.urdf.xacro b/src/rove_description/urdf/gazebo.urdf.xacro
index 161e87b..38ca36e 100644
--- a/src/rove_description/urdf/gazebo.urdf.xacro
+++ b/src/rove_description/urdf/gazebo.urdf.xacro
@@ -41,5 +41,13 @@
+
+
+ Gazebo/Grey
+ 100.0
+ 100.0
+ 1000000.0
+ 1.0
+
diff --git a/src/rove_description/urdf/rove.urdf.xacro b/src/rove_description/urdf/rove.urdf.xacro
index dec91b8..71da79b 100644
--- a/src/rove_description/urdf/rove.urdf.xacro
+++ b/src/rove_description/urdf/rove.urdf.xacro
@@ -1,63 +1,145 @@
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
-
+
+ -->
-
+
\ No newline at end of file
diff --git a/src/rove_navigation/rove_navigation/model_attacher.py b/src/rove_navigation/rove_navigation/model_attacher.py
new file mode 100755
index 0000000..5b293ba
--- /dev/null
+++ b/src/rove_navigation/rove_navigation/model_attacher.py
@@ -0,0 +1,64 @@
+import rclpy
+from rclpy.node import Node
+from ros_gz_interfaces.srv import SpawnEntity
+
+class ModelAttacherNode(Node):
+ def __init__(self):
+ super().__init__('model_attacher_node')
+
+ self.declare_parameter('parent_model', 'rove')
+ self.declare_parameter('child_model', 'ovis')
+ self.declare_parameter('parent_link', 'base_link')
+ self.declare_parameter('child_link', 'base_link')
+
+ self.parent_model = self.get_parameter('parent_model').get_parameter_value().string_value
+ self.child_model = self.get_parameter('child_model').get_parameter_value().string_value
+ self.parent_link = self.get_parameter('parent_link').get_parameter_value().string_value
+ self.child_link = self.get_parameter('child_link').get_parameter_value().string_value
+
+ # Create service client
+ self.spawn_entity_client = self.create_client(
+ SpawnEntity,
+ '/world/default/create'
+ )
+
+ while not self.spawn_entity_client.wait_for_service(timeout_sec=1.0):
+ self.get_logger().info('Waiting for SpawnEntity service...')
+
+ self.attach_models()
+
+ def attach_models(self):
+ # Create an SDF string for a fixed joint
+ joint_sdf = f"""
+
+
+
+
+ {self.parent_model}::{self.parent_link}
+ {self.child_model}::{self.child_link}
+
+
+
+ """
+
+ request = SpawnEntity.Request()
+ request.xml = joint_sdf
+ request.name = "joint_attachment"
+
+ future = self.spawn_entity_client.call_async(request)
+ rclpy.spin_until_future_complete(self, future)
+
+ if future.result() is not None:
+ self.get_logger().info('Successfully created joint between models')
+ else:
+ self.get_logger().error('Failed to create joint between models')
+
+def main(args=None):
+ rclpy.init(args=args)
+ node = ModelAttacherNode()
+ rclpy.spin(node)
+ node.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/src/rove_navigation/rove_navigation/ovis_pull.py b/src/rove_navigation/rove_navigation/ovis_pull.py
new file mode 100755
index 0000000..21f2142
--- /dev/null
+++ b/src/rove_navigation/rove_navigation/ovis_pull.py
@@ -0,0 +1,71 @@
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Pose
+from ros_gz_interfaces.srv import SetEntityPose, SpawnEntity
+import tf2_ros
+
+class PositionLockNode(Node):
+ def __init__(self):
+ super().__init__('position_lock_node')
+
+ self.declare_parameter('rove_name', 'rove')
+ self.declare_parameter('ovis_name', 'ovis')
+ self.declare_parameter('update_rate', 10.0) # Hz
+ self.declare_parameter('offset_x', 0.25)
+ self.declare_parameter('offset_y', 0.0)
+ self.declare_parameter('offset_z', 0.25)
+
+ self.rove_name = self.get_parameter('rove_name').get_parameter_value().string_value
+ self.ovis_name = self.get_parameter('ovis_name').get_parameter_value().string_value
+ self.update_rate = self.get_parameter('update_rate').get_parameter_value().double_value
+ self.offset_x = self.get_parameter('offset_x').get_parameter_value().double_value
+ self.offset_y = self.get_parameter('offset_y').get_parameter_value().double_value
+ self.offset_z = self.get_parameter('offset_z').get_parameter_value().double_value
+
+ # Set up the TF listener
+ self.tf_buffer = tf2_ros.Buffer()
+ self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
+
+ # Set up the SetEntityPose service client
+ self.set_entity_pose_client = self.create_client(SetEntityPose, '/world/default/set_pose')
+ while not self.set_entity_pose_client.wait_for_service(timeout_sec=1.0):
+ self.get_logger().info('Waiting for SetEntityPose service...')
+
+ # Timer to periodically update the position
+ self.timer = self.create_timer(1.0 / self.update_rate, self.update_position)
+
+ def update_position(self):
+ try:
+ # Look up the latest transform for `rove`
+ transform = self.tf_buffer.lookup_transform('world', 'base_link', rclpy.time.Time())
+ # Calculate the target pose for `ovis` with the given offsets
+ target_pose = Pose()
+ target_pose.position.x = transform.transform.translation.x + self.offset_x
+ target_pose.position.y = transform.transform.translation.y + self.offset_y
+ target_pose.position.z = transform.transform.translation.z + self.offset_z
+
+ # Use the quaternion orientation from `rove` directly
+ target_pose.orientation = transform.transform.rotation
+
+ # Set the new pose of `ovis`
+ set_pose_request = SetEntityPose.Request()
+ set_pose_request.name = self.ovis_name
+ set_pose_request.pose = target_pose
+ future = self.set_entity_pose_client.call_async(set_pose_request)
+ rclpy.spin_until_future_complete(self, future)
+ if future.result() is None or not future.result().success:
+ self.get_logger().error(f'Failed to set pose of {self.ovis_name}')
+ else:
+ self.get_logger().info(f'Successfully updated pose of {self.ovis_name}')
+ except Exception as e:
+ self.get_logger().error(f'Error retrieving transform: {e}')
+
+def main(args=None):
+ rclpy.init(args=args)
+ node = PositionLockNode()
+ rclpy.spin(node)
+ node.destroy_node()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
diff --git a/src/rove_navigation/setup.py b/src/rove_navigation/setup.py
index 9801881..f3d2c1a 100644
--- a/src/rove_navigation/setup.py
+++ b/src/rove_navigation/setup.py
@@ -32,6 +32,8 @@
'exploration = rove_navigation.exploration:main',
'lost_connection = rove_navigation.lost_connection:main',
'mule_behavior = rove_navigation.mule_behavior:main',
+ 'ovis_pull = rove_navigation.ovis_pull:main',
+ 'model_attacher = rove_navigation.model_attacher:main',
],
},
)