Skip to content

Commit

Permalink
it works ! long live the friction
Browse files Browse the repository at this point in the history
  • Loading branch information
@SimonR99
SimonR99 committed Nov 14, 2024
1 parent 023f190 commit a8f8e04
Show file tree
Hide file tree
Showing 7 changed files with 252 additions and 38 deletions.
4 changes: 2 additions & 2 deletions src/rove_bringup/launch/common.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
])
19 changes: 3 additions & 16 deletions src/rove_bringup/launch/sim.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand All @@ -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,
Expand All @@ -174,6 +162,5 @@ def generate_launch_description():
common,
# human_tracker,
ovis,
static_tf_ovis,
]
)
8 changes: 8 additions & 0 deletions src/rove_description/urdf/gazebo.urdf.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,5 +41,13 @@
</plugin>

<plugin filename="gz-sim-navsat-system" name="gz::sim::systems::NavSat"/>

<gazebo reference="friction_plate_base">
<material>Gazebo/Grey</material>
<mu1>100.0</mu1>
<mu2>100.0</mu2>
<kp>1000000.0</kp>
<kd>1.0</kd>
</gazebo>
</gazebo>
</robot>
122 changes: 102 additions & 20 deletions src/rove_description/urdf/rove.urdf.xacro
View file
Original file line number Diff line number Diff line change
@@ -1,63 +1,145 @@
<?xml version="1.0"?>
<robot name="rove" xmlns:xacro="http://www.ros.org/wiki/xacro">
<material name="rubber_color">
<color rgba="0.1 0.1 0.1 1.0"/>
<color rgba="0.1 0.1 0.1 1.0" />
</material>
<material name="frame_color">
<color rgba="0.5 0.5 0.5 1.0"/>
<color rgba="0.5 0.5 0.5 1.0" />
</material>
<material name="lidar_color">
<color rgba="1 0 0 1.0"/>
<color rgba="1 0 0 1.0" />
</material>

<xacro:include filename="$(find rove_description)/urdf/gazebo.urdf.xacro" />
<xacro:include filename="$(find rove_description)/urdf/sensor.urdf.xacro" />

<xacro:include filename="$(find rove_description)/urdf/flipper.urdf.xacro" />
<xacro:include filename="$(find rove_description)/urdf/track.urdf.xacro" />
<xacro:include filename="$(find rove_description)/urdf/rove.ros2_control.urdf.xacro" />

<!-- <xacro:include filename="$(find ovis_description)/urdf/ovis.urdf.xacro" /> -->

<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="https://raw.githubusercontent.com/clubcapra/rove/main/src/rove_description/meshes/base.stl"/>
<mesh
filename="https://raw.githubusercontent.com/clubcapra/rove/main/src/rove_description/meshes/base.stl" />
</geometry>
<material name="frame_color"/>
<material name="frame_color" />
</visual>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://$(find rove_description)/meshes/base.stl"/>
<mesh filename="file://$(find rove_description)/meshes/base.stl" />
</geometry>
<material name="frame_color"/>
<material name="frame_color" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://$(find rove_description)/meshes/base.stl"/>
<mesh filename="file://$(find rove_description)/meshes/base.stl" />
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
<mass value="80"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
<origin xyz="0 0 0.5" rpy="0 0 0" />
<mass value="80" />
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1" />
</inertial>
</link>

<link name="friction_plate_base">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.3 0.3 0.01" />
</geometry>
<material name="frame_color" />
</visual>
<visual>
<origin xyz="0.14 0 0.02" rpy="0 0 0" />
<geometry>
<box size="0.02 0.3 0.04" />
</geometry>
<material name="frame_color" />
</visual>
<visual>
<origin xyz="-0.14 0 0.02" rpy="0 0 0" />
<geometry>
<box size="0.02 0.3 0.04" />
</geometry>
<material name="frame_color" />
</visual>
<visual>
<origin xyz="0 0.14 0.02" rpy="0 0 0" />
<geometry>
<box size="0.26 0.02 0.04" />
</geometry>
<material name="frame_color" />
</visual>
<visual>
<origin xyz="0 -0.14 0.02" rpy="0 0 0" />
<geometry>
<box size="0.26 0.02 0.04" />
</geometry>
<material name="frame_color" />
</visual>

<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.3 0.3 0.01" />
</geometry>
</collision>
<collision>
<origin xyz="0.14 0 0.02" rpy="0 0 0" />
<geometry>
<box size="0.02 0.3 0.04" />
</geometry>
</collision>
<collision>
<origin xyz="-0.14 0 0.02" rpy="0 0 0" />
<geometry>
<box size="0.02 0.3 0.04" />
</geometry>
</collision>
<collision>
<origin xyz="0 0.14 0.02" rpy="0 0 0" />
<geometry>
<box size="0.26 0.02 0.04" />
</geometry>
</collision>
<collision>
<origin xyz="0 -0.14 0.02" rpy="0 0 0" />
<geometry>
<box size="0.26 0.02 0.04" />
</geometry>
</collision>

<inertial>
<mass value="1.5" />
<inertia ixx="0.02" ixy="0" ixz="0" iyy="0.02" iyz="0" izz="0.02" />
</inertial>
</link>

<joint name="friction_plate_base_joint" type="fixed">
<parent link="base_link" />
<child link="friction_plate_base" />
<origin xyz="0.22 0 0.2" rpy="0 0 0" />
</joint>

<!-- <xacro:rove_ros2_control/> -->
<!-- Put these at -1 to mock the motors if working without the motors -->
<!-- Put these at -1 to mock the motors if working without the motors -->
<!-- <xacro:odrive_control fl_id="21" rl_id="22" fr_id="23" rr_id="24"/> -->
<!-- TODO Fix rr Odrive, it keeps crashing the rest of the drives. IT has been disabled for now. -->
<xacro:odrive_control fl_id="21" rl_id="22" fr_id="23" rr_id="24"/>
<xacro:odrive_control fl_id="21" rl_id="22" fr_id="23" rr_id="24" />


<!-- Insert ovis on top of rove -->
<!-- <xacro:property name="robot_root" value="base_link" />
<xacro:property name="ovis_robot_namespace" value="rove" />
<xacro:property name="ovis_base_origin" value="-0.2 0 0.2" />
<xacro:ovis base_parent="${robot_root}" prefix="${ovis_robot_namespace}" joint_base_origin_xyz="${ovis_base_origin}" /> -->
<xacro:ovis base_parent="${robot_root}" prefix="${ovis_robot_namespace}"
joint_base_origin_xyz="${ovis_base_origin}" /> -->

</robot>
</robot>
64 changes: 64 additions & 0 deletions src/rove_navigation/rove_navigation/model_attacher.py
View file
Original file line number Diff line number Diff line change
@@ -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"""
<?xml version="1.0" ?>
<sdf version="1.8">
<model name="joint_attachment">
<joint name="rove_ovis_joint" type="fixed">
<parent>{self.parent_model}::{self.parent_link}</parent>
<child>{self.child_model}::{self.child_link}</child>
</joint>
</model>
</sdf>
"""

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()
71 changes: 71 additions & 0 deletions src/rove_navigation/rove_navigation/ovis_pull.py
View file
Original file line number Diff line number Diff line change
@@ -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()
2 changes: 2 additions & 0 deletions src/rove_navigation/setup.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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',
],
},
)

0 comments on commit a8f8e04

Please sign in to comment.