Skip to content

Commit

Permalink
cleanup
Browse files Browse the repository at this point in the history
  • Loading branch information
D-1shu authored and ggupta9777 committed Jun 28, 2023
1 parent 23d90b7 commit 5936a73
Show file tree
Hide file tree
Showing 3 changed files with 70 additions and 64 deletions.
27 changes: 17 additions & 10 deletions launch/gazebo.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -18,37 +18,44 @@ def get_xacro_to_doc(xacro_file_path, mappings):
return doc

def generate_launch_description():
# Retrieve launch configuration arguments
use_sim_time = LaunchConfiguration('use_sim_time', default='true')

use_sim_time = LaunchConfiguration('use_sim_time', default='false')
# Path to the Xacro file
xacro_path = join(get_package_share_directory('new_bcr_robot'), 'urdf', 'new_bcr_robot.xacro')
doc = get_xacro_to_doc(xacro_path, {"wheel_odom_topic": "odom", "sim_gazebo": "true"})

# Launch the robot_state_publisher node
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
output='screen',
parameters=[{'use_sim_time': use_sim_time}, {'robot_description': doc.toxml()}]
)

spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
# Launch the spawn_entity node to spawn the robot in Gazebo
spawn_entity = Node(
package='gazebo_ros',
executable='spawn_entity.py',
arguments=['-topic', 'robot_description', '-entity', 'new_bcr_robot'],
output='screen')
output='screen'
)

# Include the Gazebo launch file
gazebo_share = get_package_share_directory("gazebo_ros")
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(join(gazebo_share, "launch", "gazebo.launch.py"))
)

return LaunchDescription([
DeclareLaunchArgument(
'world', default_value=[FindPackageShare('new_bcr_robot'), '/worlds/gazebo/mars.world'],
),
# Declare launch arguments
DeclareLaunchArgument('world', default_value=[FindPackageShare('new_bcr_robot'), '/worlds/gazebo/mars.world']),
DeclareLaunchArgument('gui', default_value='true'),
DeclareLaunchArgument('verbose', default_value='false'),
DeclareLaunchArgument('use_sim_time', default_value='false'),
DeclareLaunchArgument('use_sim_time', default_value='true'),
DeclareLaunchArgument('robot_description', default_value=doc.toxml()),
gazebo,
robot_state_publisher,
spawn_entity
])
])
106 changes: 52 additions & 54 deletions urdf/gazebo.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -2,16 +2,15 @@
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="new_bcr_robot">

<gazebo>
<static>false</static>
<static>false</static>
</gazebo>


<!-- .....................MULTI WHEEL DIFF DRIVE ................................... -->

<gazebo>
<plugin name="diff_drive" filename="libgazebo_ros_diff_drive.so">
<legacy_mode>false</legacy_mode>
<update_rate>50.0</update_rate>
<legacy_mode>false</legacy_mode>
<update_rate>50.0</update_rate>
<left_joint>middle_left_wheel_joint</left_joint>
<right_joint>middle_right_wheel_joint</right_joint>
<wheel_separation>${traction_wheel_base+traction_wheel_width-0.01}</wheel_separation>
Expand All @@ -21,20 +20,18 @@
<command_topic>cmd_vel</command_topic>
<odometry_topic>$(arg wheel_odom_topic)</odometry_topic>
<odometry_frame>odom</odometry_frame>
<xacro:unless value="$(arg publish_wheel_odom_tf)">
<publish_odom_tf>false</publish_odom_tf>
</xacro:unless>
<publish_odom_tf>$(arg publish_wheel_odom_tf)</publish_odom_tf>
<publish_wheel_tf>false</publish_wheel_tf>
<!-- <publish_wheel_joint_state>true</publish_wheel_joint_state> -->
<publish_odom>true</publish_odom>
<!-- <odometry_source>encoder</odometry_source> -->
<max_wheel_acceleration>5.0</max_wheel_acceleration>
<max_wheel_acceleration>5.0</max_wheel_acceleration>
</plugin>
</gazebo>

<!--............................... IMU PLUGIN ..................................... -->

<!-- <gazebo>
<!--
<gazebo>
<plugin name="imu_plugin" filename="libgazebo_ros_imu.so">
<updateRate>50.0</updateRate>
<bodyName>base_link</bodyName>
Expand All @@ -57,11 +54,13 @@
<gaussianNoise>0.0</gaussianNoise>
<frameName>imu_frame</frameName>
</plugin>
</gazebo> -->
</gazebo>
-->

<!--............................... Ground truth PLUGIN .............................-->

<!-- <gazebo>

<gazebo>
<plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
<always_on>true</always_on>
<update_rate>30.0</update_rate>
Expand All @@ -70,45 +69,45 @@
<gaussian_noise>0.00</gaussian_noise>
<frame_name>$(arg ground_truth_frame)</frame_name>
</plugin>
</gazebo> -->
</gazebo>

<!-- ........................... 2D LIDAR PLUGIN ................................... -->

<xacro:if value="$(arg two_d_lidar_enabled)">

<gazebo reference="two_d_lidar">
<gravity>true</gravity>
<sensor type="ray" name="two_d_lidar">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>${two_d_lidar_update_rate}</update_rate>
<ray>
<scan>
<horizontal>
<samples>${two_d_lidar_sample_size}</samples>
<resolution>1</resolution>
<min_angle>${radians(two_d_lidar_min_angle)}</min_angle>
<max_angle>${radians(two_d_lidar_max_angle)}</max_angle>
</horizontal>
</scan>
<range>
<min>${two_d_lidar_min_range}</min>
<max>${two_d_lidar_max_range}</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.001</stddev>
</noise>
</ray>
<plugin name="gazebo_ros_laser" filename="libgazebo_ros_laser.so">
<topicName>scan</topicName>
<frameName>two_d_lidar</frameName>
<robotNamespace>/</robotNamespace>
</plugin>
</sensor>
</gazebo>
<gazebo reference="two_d_lidar">
<gravity>true</gravity>
<sensor type="ray" name="two_d_lidar">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>${two_d_lidar_update_rate}</update_rate>
<ray>
<scan>
<horizontal>
<samples>${two_d_lidar_sample_size}</samples>
<resolution>1</resolution>
<min_angle>${radians(two_d_lidar_min_angle)}</min_angle>
<max_angle>${radians(two_d_lidar_max_angle)}</max_angle>
</horizontal>
</scan>
<range>
<min>${two_d_lidar_min_range}</min>
<max>${two_d_lidar_max_range}</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.001</stddev>
</noise>
</ray>
<plugin name="gazebo_ros_laser" filename="libgazebo_ros_laser.so">
<topicName>scan</topicName>
<frameName>two_d_lidar</frameName>
<robotNamespace>$(arg robot_namespace)</robotNamespace>
</plugin>
</sensor>
</gazebo>

</xacro:if>

Expand All @@ -124,13 +123,13 @@
<camera>
<horizontal_fov>${radians(camera_horizontal_fov)}</horizontal_fov>
<image>
<format>R8G8B8</format>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.05</near>
<far>8.0</far>
<near>0.05</near>
<far>8.0</far>
</clip>
</camera>
<plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
Expand All @@ -150,7 +149,7 @@
<distortion_t1>0.0</distortion_t1>
<distortion_t2>0.0</distortion_t2>
<pointCloudCutoff>0.4</pointCloudCutoff>
<robotNamespace>/</robotNamespace>
<robotNamespace>$(arg robot_namespace)</robotNamespace>
</plugin>
</sensor>
</gazebo>
Expand All @@ -159,5 +158,4 @@

<!--................................................................................. -->


</robot>
</robot>
1 change: 1 addition & 0 deletions urdf/new_bcr_robot.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
<xacro:property name="camera_height" value="0.10"/>
<xacro:property name="camera_horizontal_fov" value="60"/>

<xacro:arg name="robot_namespace" default="/"/>
<xacro:arg name="wheel_odom_topic" default="odom" />
<xacro:arg name="camera_enabled" default="false" />
<xacro:arg name="two_d_lidar_enabled" default="false" />
Expand Down

0 comments on commit 5936a73

Please sign in to comment.