Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
29 changes: 29 additions & 0 deletions crazyflie_ws/src/crazyflie_description/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,29 @@
cmake_minimum_required(VERSION 3.8)
project(crazyflie_description)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)


if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
set(ament_cmake_copyright_FOUND TRUE)
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()

# Install launch files
install(DIRECTORY
launch
urdf
meshes
rviz
config
DESTINATION share/${PROJECT_NAME}/
)

ament_package()
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
---
- ros_topic_name: "/cmd_vel"
gz_topic_name: "/crazyflie/gazebo/command/twist"
ros_type_name: "geometry_msgs/msg/Twist"
gz_type_name: "ignition.msgs.Twist"
direction: ROS_TO_GZ
- ros_topic_name: "/clock"
gz_topic_name: "/clock"
ros_type_name: "rosgraph_msgs/msg/Clock"
gz_type_name: "gz.msgs.Clock"
Original file line number Diff line number Diff line change
@@ -0,0 +1,65 @@
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
import os
import xacro


def generate_launch_description():
# Package and xacro path
pkg_share = get_package_share_directory("crazyflie_description")
xacro_file = os.path.join(pkg_share, "urdf", "crazyflie_body.xacro")

# Process xacro → urdf
robot_description_config = xacro.process_file(xacro_file).toxml()

# Launch argument for robot name
robot_name_arg = DeclareLaunchArgument(
"robot_name",
default_value="crazyflie",
description="Name of the robot"
)

# Start Gazebo Harmonic (empty world)
gazebo_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(
get_package_share_directory("ros_gz_sim"), "launch", "gz_sim.launch.py"
)
),
launch_arguments={"gz_args": "-r empty.sdf"}.items(),
)

# Spawn robot in Gazebo
spawn_robot = Node(
package="ros_gz_sim",
executable="create",
arguments=[
"-name", LaunchConfiguration("robot_name"),
"-string", robot_description_config,
"-x", "0.0", # X position
"-y", "0.0", # Y position
"-z", "0.5", # Z position (height)
],
output="screen",
)

# ROS-Gazebo bridge
bridge = Node(
package="ros_gz_bridge",
executable="parameter_bridge",
parameters=[{
"config_file": os.path.join(pkg_share, "config", "ros_gz_crazyflie_bridge.yaml"),
}],
output="screen",
)

return LaunchDescription([
robot_name_arg,
gazebo_launch,
spawn_robot,
bridge,
])
Original file line number Diff line number Diff line change
@@ -0,0 +1,74 @@
from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import Command, PathJoinSubstitution
from launch.substitutions.launch_configuration import LaunchConfiguration

from launch_ros.actions import Node


ARGUMENTS = [
DeclareLaunchArgument('use_sim_time', default_value='false',
choices=['true', 'false'],
description='Use simulation (Gazebo) clock if true'),
DeclareLaunchArgument('robot_name', default_value='crazyflie',
description='Robot name'),
DeclareLaunchArgument('namespace', default_value=LaunchConfiguration('robot_name'),
description='Robot namespace'),
]


def generate_launch_description():
pkg_crazyflie_description = get_package_share_directory('crazyflie_description')

# Paths
xacro_file = PathJoinSubstitution([pkg_crazyflie_description, 'urdf', 'crazyflie_body.xacro'])
rviz_config_file = PathJoinSubstitution([pkg_crazyflie_description, 'rviz', 'crazyflie_body.rviz'])

namespace = LaunchConfiguration('namespace')

# Robot State Publisher
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
output='screen',
parameters=[
{'use_sim_time': LaunchConfiguration('use_sim_time')},
{'robot_description': Command(['xacro ', xacro_file])},
],
remappings=[
('/tf', 'tf'),
('/tf_static', 'tf_static')
]
)

# Joint State Publisher GUI (for revolute joints)
joint_state_publisher_gui = Node(
package='joint_state_publisher_gui',
executable='joint_state_publisher_gui',
name='joint_state_publisher_gui',
output='screen',
parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}],
remappings=[
('/tf', 'tf'),
('/tf_static', 'tf_static')
]
)

# RViz2
rviz2 = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', rviz_config_file],
output='screen'
)

# LaunchDescription
ld = LaunchDescription(ARGUMENTS)
ld.add_action(robot_state_publisher)
ld.add_action(joint_state_publisher_gui)
ld.add_action(rviz2)
return ld

Large diffs are not rendered by default.

Large diffs are not rendered by default.

Loading