This commit is contained in:
yikestone 2018-05-27 22:08:36 +05:30
commit e3894ca3f9
17 changed files with 1002 additions and 0 deletions

View File

@ -0,0 +1,13 @@
<?xml version="1.0"?>
<model>
<name>line_follower</name>
<version>1.0</version>
<sdf>urdf/line_follower.urdf</sdf>
<author>
<name>yikestone</name>
<email>name@email.address</email>
</author>
<description>
A description of the model
</description>
</model>

View File

@ -0,0 +1,13 @@
cmake_minimum_required(VERSION 2.8.3)
project(line_follower_description)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
roscpp
rviz
tf
urdf
xacro
)
include_directories(
${catkin_INCLUDE_DIRS}
)

View File

@ -0,0 +1,16 @@
line_follower:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
# Effort Controllers ---------------------------------------
leftWheel_effort_controller:
type: effort_controllers/JointEffortController
joint: left_wheel_hinge
pid: {p: 100.0, i: 0.1, d: 10.0}
rightWheel_effort_controller:
type: effort_controllers/JointEffortController
joint: right_wheel_hinge
pid: {p: 100.0, i: 0.1, d: 10.0}

View File

@ -0,0 +1,12 @@
<launch>
<arg name="model" />
<param name="robot_description" command="$(find xacro)/xacro.py $(find line_follower_description)/urdf/line_follower.xacro" />
<param name="use_gui" value="true"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find line_follower_description)/urdf.rviz" required="true" />
<rosparam file="$(find line_follower_description)/config/line_follower_config.yaml" command="load"/>
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" ns="/line_follower" args="joint_state_controller rightWheel_effort_controller leftWheel_effort_controller"/>
</launch>

Binary file not shown.

View File

@ -0,0 +1,23 @@
<?xml version="1.0"?>
<package format="1">
<name>line_follower_description</name>
<version>0.0.0</version>
<description>The line_follower_description package</description>
<maintainer email="yikes@todo.todo">yikes</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rviz</build_depend>
<build_depend>tf</build_depend>
<build_depend>urdf</build_depend>
<build_depend>xacro</build_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rviz</run_depend>
<run_depend>tf</run_depend>
<run_depend>urdf</run_depend>
<run_depend>xacro</run_depend>
<export>
</export>
</package>

View File

@ -0,0 +1,194 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
Splitter Ratio: 0.5
Tree Height: 330
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.03
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
bottom_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
elbow_pitch_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
elbow_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
gripper_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
gripper_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_aft_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
shoulder_pan_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
shoulder_pitch_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wrist_pitch_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wrist_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: rviz/TF
Enabled: false
Frame Timeout: 15
Frames:
All Enabled: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
{}
Update Interval: 0
Value: false
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: base_link
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 1.12009
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: -0.138732
Y: -0.0432205
Z: 0.292796
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.230398
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.425398
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 611
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000013c000001d9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001d9000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000001d9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000001d9000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004c00000003efc0100000002fb0000000800540069006d00650100000000000004c0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000269000001d900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1216
X: 43
Y: 14

View File

@ -0,0 +1,44 @@
Background\ ColorR=0.0941176
Background\ ColorG=0
Background\ ColorB=0.466667
Fixed\ Frame=/base_link
Target\ Frame=<Fixed Frame>
Grid.Alpha=0.5
Grid.Cell\ Size=0.5
Grid.ColorR=0.898039
Grid.ColorG=0.898039
Grid.ColorB=0.898039
Grid.Enabled=1
Grid.Line\ Style=0
Grid.Line\ Width=0.03
Grid.Normal\ Cell\ Count=0
Grid.OffsetX=0
Grid.OffsetY=0
Grid.OffsetZ=0
Grid.Plane=0
Grid.Plane\ Cell\ Count=10
Grid.Reference\ Frame=<Fixed Frame>
Robot\ Model.Alpha=1
Robot\ Model.Collision\ Enabled=0
Robot\ Model.Enabled=1
Robot\ Model.Robot\ Description=robot_description
Robot\ Model.TF\ Prefix=
Robot\ Model.Update\ Interval=0
Robot\ Model.Visual\ Enabled=1
Robot\:\ Robot\ Model\ Link\ base_linkShow\ Axes=0
Robot\:\ Robot\ Model\ Link\ base_linkShow\ Trail=0
Robot\:\ Robot\ Model\ Link\ legShow\ Axes=0
Robot\:\ Robot\ Model\ Link\ legShow\ Trail=0
Tool\ 2D\ Nav\ GoalTopic=goal
Tool\ 2D\ Pose\ EstimateTopic=initialpose
Camera\ Type=rviz::OrbitViewController
Camera\ Config=1.15779 3.76081 2.16475 0 0 0
Property\ Grid\ State=selection=.Global StatusTopStatus;expanded=.Global Options,Grid.Enabled,Robot Model.Enabled;scrollpos=0,0;splitterpos=150,285;ispageselected=1
[Display0]
Name=Grid
Package=rviz
ClassName=rviz::GridDisplay
[Display1]
Name=Robot Model
Package=rviz
ClassName=rviz::RobotModelDisplay

View File

@ -0,0 +1,84 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/line_follower</robotNamespace>
</plugin>
</gazebo>
<gazebo>
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
<alwaysOn>true</alwaysOn>
<legacyMode>false</legacyMode>
<updateRate>100</updateRate>
<leftJoint>left_wheel_hinge</leftJoint>
<rightJoint>right_wheel_hinge</rightJoint>
<wheelSeparation>${chassisWidth+wheelWidth}</wheelSeparation>
<wheelDiameter>${2*wheelRadius}</wheelDiameter>
<wheelTorque>20</wheelTorque>
<commandTopic>line_follower/cmd_vel</commandTopic>
<publishWheelTF>true</publishWheelTF>
<wheelAcceleration>1</wheelAcceleration>
<odometryTopic>line_follower/odom_diffdrive</odometryTopic>
<publishWheelJointState>true</publishWheelJointState>
<odometryFrame>odom</odometryFrame>
<publishTf>true</publishTf>
<odometrySource>1</odometrySource>
<robotBaseFrame>footprint</robotBaseFrame>
</plugin>
</gazebo>
<gazebo>
<plugin name="ground_truth" filename="libgazebo_ros_p3d.so">
<frameName>map</frameName>
<bodyName>chassis</bodyName>
<topicName>line_follower/odom</topicName>
<updateRate>30.0</updateRate>
</plugin>
</gazebo>
<gazebo reference="chassis">
<material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="caster_wheel">
<mu1>0.0</mu1>
<mu2>0.0</mu2>
<material>Gazebo/Grey</material>
</gazebo>
<gazebo reference="camera">
<material>Gazebo/Grey</material>
<sensor type="camera" name="camera1">
<update_rate>30.0</update_rate>
<camera name="head">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>800</width>
<height>800</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>line_follower/camera1</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
</robot>

View File

@ -0,0 +1,270 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from line_follower.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="line_follower" xmlns:xacro="http://www.ros.org/wiki/xacro">
<gazebo>
<plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
<robotNamespace>/line_follower</robotNamespace>
</plugin>
</gazebo>
<gazebo>
<plugin filename="libgazebo_ros_diff_drive.so" name="differential_drive_controller">
<alwaysOn>true</alwaysOn>
<legacyMode>false</legacyMode>
<updateRate>100</updateRate>
<leftJoint>left_wheel_hinge</leftJoint>
<rightJoint>right_wheel_hinge</rightJoint>
<wheelSeparation>0.25</wheelSeparation>
<wheelDiameter>0.2</wheelDiameter>
<wheelTorque>20</wheelTorque>
<commandTopic>line_follower/cmd_vel</commandTopic>
<publishWheelTF>true</publishWheelTF>
<wheelAcceleration>1</wheelAcceleration>
<odometryTopic>line_follower/odom_diffdrive</odometryTopic>
<publishWheelJointState>true</publishWheelJointState>
<odometryFrame>odom</odometryFrame>
<publishTf>true</publishTf>
<odometrySource>1</odometrySource>
<robotBaseFrame>footprint</robotBaseFrame>
</plugin>
</gazebo>
<gazebo>
<plugin filename="libgazebo_ros_p3d.so" name="ground_truth">
<frameName>map</frameName>
<bodyName>chassis</bodyName>
<topicName>line_follower/odom</topicName>
<updateRate>30.0</updateRate>
</plugin>
</gazebo>
<gazebo reference="chassis">
<material>Gazebo/Orange</material>
</gazebo>
<gazebo reference="caster_wheel">
<mu1>0.0</mu1>
<mu2>0.0</mu2>
<material>Gazebo/Grey</material>
</gazebo>
<gazebo reference="camera">
<material>Gazebo/Grey</material>
<sensor name="camera1" type="camera">
<update_rate>30.0</update_rate>
<camera name="head">
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>800</width>
<height>800</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
</camera>
<plugin filename="libgazebo_ros_camera.so" name="camera_controller">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>line_follower/camera1</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="orange">
<color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
</material>
<material name="brown">
<color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<link name="footprint"/>
<joint name="base_joint" type="fixed">
<parent link="footprint"/>
<child link="chassis"/>
</joint>
<link name="chassis">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.1"/>
<geometry>
<box size="0.4 0.2 0.1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.1"/>
<geometry>
<box size="0.4 0.2 0.1"/>
</geometry>
<material name="orange"/>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.1"/>
<mass value="1"/>
<inertia ixx="0.00416666666667" ixy="0" ixz="0" iyy="0.0141666666667" iyz="0" izz="0.0141666666667"/>
</inertial>
</link>
<joint name="fixed" type="fixed">
<parent link="chassis"/>
<child link="caster_wheel"/>
</joint>
<link name="caster_wheel">
<collision>
<origin rpy="0 0 0" xyz="-0.15 0 0.05"/>
<geometry>
<sphere radius="0.05"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="-0.15 0 0.05"/>
<geometry>
<sphere radius="0.05"/>
</geometry>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="-0.15 0 0.05"/>
<mass value="0.5"/>
<inertia ixx="0.0005" ixy="0" ixz="0" iyy="0.0005" iyz="0" izz="0.0005"/>
</inertial>
</link>
<link name="left_wheel">
<collision>
<origin rpy="0 1.57079632679 1.57079632679" xyz="0 0 0"/>
<geometry>
<cylinder length="0.05" radius="0.1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 1.57079632679 1.57079632679" xyz="0 0 0"/>
<geometry>
<cylinder length="0.05" radius="0.1"/>
</geometry>
<material name="black"/>
</visual>
<inertial>
<origin rpy="0 1.57079632679 1.57079632679" xyz="0 0 0"/>
<mass value="0.2"/>
<inertia ixx="0.000541666666667" ixy="0" ixz="0" iyy="0.000541666666667" iyz="0" izz="0.001"/>
</inertial>
</link>
<gazebo reference="left_wheel">
<mu1 value="1.0"/>
<mu2 value="1.0"/>
<kp value="10000000.0"/>
<kd value="1.0"/>
<fdir1 value="1 0 0"/>
<material>Gazebo/Black</material>
</gazebo>
<joint name="left_wheel_hinge" type="continuous">
<parent link="chassis"/>
<child link="left_wheel"/>
<origin rpy="0 0 0" xyz="0.1 -0.125 0.1"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<transmission name="left_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="left_wheel_hinge">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="leftMotor">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>10</mechanicalReduction>
</actuator>
</transmission>
<link name="right_wheel">
<collision>
<origin rpy="0 1.57079632679 1.57079632679" xyz="0 0 0"/>
<geometry>
<cylinder length="0.05" radius="0.1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 1.57079632679 1.57079632679" xyz="0 0 0"/>
<geometry>
<cylinder length="0.05" radius="0.1"/>
</geometry>
<material name="black"/>
</visual>
<inertial>
<origin rpy="0 1.57079632679 1.57079632679" xyz="0 0 0"/>
<mass value="0.2"/>
<inertia ixx="0.000541666666667" ixy="0" ixz="0" iyy="0.000541666666667" iyz="0" izz="0.001"/>
</inertial>
</link>
<gazebo reference="right_wheel">
<mu1 value="1.0"/>
<mu2 value="1.0"/>
<kp value="10000000.0"/>
<kd value="1.0"/>
<fdir1 value="1 0 0"/>
<material>Gazebo/Black</material>
</gazebo>
<joint name="right_wheel_hinge" type="continuous">
<parent link="chassis"/>
<child link="right_wheel"/>
<origin rpy="0 0 0" xyz="0.1 0.125 0.1"/>
<axis rpy="0 0 0" xyz="0 1 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<transmission name="right_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="right_wheel_hinge">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="rightMotor">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>10</mechanicalReduction>
</actuator>
</transmission>
<joint name="camera_joint" type="fixed">
<origin rpy="0 0.60 0" xyz="0.2 0 0.175"/>
<parent link="chassis"/>
<child link="camera"/>
</joint>
<link name="camera">
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
<material name="blue"/>
</visual>
<inertial>
<mass value="0.01"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="4.16666666667e-06" ixy="0" ixz="0" iyy="4.16666666667e-06" iyz="0" izz="4.16666666667e-06"/>
</inertial>
</link>
</robot>

View File

@ -0,0 +1,119 @@
<?xml version="1.0"?>
<robot name="line_follower" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:property name="PI" value="3.1415926535897931"/>
<xacro:property name="chassisHeight" value="0.1"/>
<xacro:property name="chassisLength" value="0.4"/>
<xacro:property name="chassisWidth" value="0.2"/>
<xacro:property name="chassisMass" value="1"/>
<xacro:property name="casterRadius" value="0.05"/>
<xacro:property name="casterMass" value="0.5"/>
<xacro:property name="wheelWidth" value="0.05"/>
<xacro:property name="wheelRadius" value="0.1"/>
<xacro:property name="wheelPos" value="0.2"/>
<xacro:property name="wheelMass" value="0.2"/>
<xacro:property name="cameraSize" value="0.05"/>
<xacro:property name="cameraMass" value="0.01"/>
<xacro:include filename="$(find line_follower_description)/urdf/line_follower.gazebo" />
<xacro:include filename="$(find line_follower_description)/urdf/materials.xacro" />
<xacro:include filename="$(find line_follower_description)/urdf/macros.xacro" />
<link name="footprint" />
<joint name="base_joint" type="fixed">
<parent link="footprint"/>
<child link="chassis"/>
</joint>
<link name="chassis">
<collision>
<origin xyz="0 0 ${wheelRadius}" rpy="0 0 0"/>
<geometry>
<box size="${chassisLength} ${chassisWidth} ${chassisHeight}"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 ${wheelRadius}" rpy="0 0 0"/>
<geometry>
<box size="${chassisLength} ${chassisWidth} ${chassisHeight}"/>
</geometry>
<material name="orange"/>
</visual>
<inertial>
<origin xyz="0 0 ${wheelRadius}" rpy="0 0 0"/>
<mass value="${chassisMass}"/>
<box_inertia m="${chassisMass}" x="${chassisLength}" y="${chassisWidth}" z="${chassisHeight}"/>
</inertial>
</link>
<joint name="fixed" type="fixed">
<parent link="chassis"/>
<child link="caster_wheel"/>
</joint>
<link name="caster_wheel">
<collision>
<origin xyz="${casterRadius-chassisLength/2} 0 ${casterRadius-chassisHeight+wheelRadius}" rpy="0 0 0"/>
<geometry>
<sphere radius="${casterRadius}"/>
</geometry>
</collision>
<visual>
<origin xyz="${casterRadius-chassisLength/2} 0 ${casterRadius-chassisHeight+wheelRadius}" rpy="0 0 0"/>
<geometry>
<sphere radius="${casterRadius}"/>
</geometry>
</visual>
<inertial>
<origin xyz="${casterRadius-chassisLength/2} 0 ${casterRadius-chassisHeight+wheelRadius}" rpy="0 0 0"/>
<mass value="${casterMass}"/>
<sphere_inertia m="${casterMass}" r="${casterRadius}"/>
</inertial>
</link>
<wheel lr="left" tY="-1"/>
<wheel lr="right" tY="1"/>
<joint name="camera_joint" type="fixed">
<origin xyz="${chassisLength/2} 0 ${cameraSize+wheelRadius+cameraSize/2}" rpy="0 0.60 0"/>
<parent link="chassis"/>
<child link="camera"/>
</joint>
<link name="camera">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="${cameraSize} ${cameraSize} ${cameraSize}"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="${cameraSize} ${cameraSize} ${cameraSize}"/>
</geometry>
<material name="blue"/>
</visual>
<inertial>
<mass value="${cameraMass}" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<box_inertia m="${cameraMass}" x="${cameraSize}" y="${cameraSize}" z="${cameraSize}" />
</inertial>
</link>
</robot>

View File

@ -0,0 +1,78 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="cylinder_inertia" params="m r h">
<inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
izz="${m*r*r/2}" />
</xacro:macro>
<xacro:macro name="box_inertia" params="m x y z">
<inertia ixx="${m*(y*y+z*z)/12}" ixy = "0" ixz = "0"
iyy="${m*(x*x+z*z)/12}" iyz = "0"
izz="${m*(x*x+z*z)/12}" />
</xacro:macro>
<xacro:macro name="sphere_inertia" params="m r">
<inertia ixx="${2*m*r*r/5}" ixy = "0" ixz = "0"
iyy="${2*m*r*r/5}" iyz = "0"
izz="${2*m*r*r/5}" />
</xacro:macro>
<xacro:macro name="wheel" params="lr tY">
<link name="${lr}_wheel">
<collision>
<origin xyz="0 0 0" rpy="0 ${PI/2} ${PI/2}" />
<geometry>
<cylinder length="${wheelWidth}" radius="${wheelRadius}"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 ${PI/2} ${PI/2}" />
<geometry>
<cylinder length="${wheelWidth}" radius="${wheelRadius}"/>
</geometry>
<material name="black"/>
</visual>
<inertial>
<origin xyz="0 0 0" rpy="0 ${PI/2} ${PI/2}" />
<mass value="${wheelMass}"/>
<cylinder_inertia m="${wheelMass}" r="${wheelRadius}" h="${wheelWidth}"/>
</inertial>
</link>
<gazebo reference="${lr}_wheel">
<mu1 value="1.0"/>
<mu2 value="1.0"/>
<kp value="10000000.0" />
<kd value="1.0" />
<fdir1 value="1 0 0"/>
<material>Gazebo/Black</material>
</gazebo>
<joint name="${lr}_wheel_hinge" type="continuous">
<parent link="chassis"/>
<child link="${lr}_wheel"/>
<origin xyz="${+wheelPos-chassisLength/4} ${tY*wheelWidth/2+tY*chassisWidth/2} ${wheelRadius}" rpy="0 0 0" />
<axis xyz="0 1 0" rpy="0 0 0" />
<limit effort="100" velocity="100"/>
<joint_properties damping="0.0" friction="0.0"/>
</joint>
<transmission name="${lr}_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${lr}_wheel_hinge">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="${lr}Motor">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>10</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
</robot>

View File

@ -0,0 +1,36 @@
<?xml version="1.0"?>
<robot>
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="orange">
<color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
</material>
<material name="brown">
<color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
</robot>

View File

@ -0,0 +1,24 @@
cmake_minimum_required(VERSION 2.8.3)
project(line_follower_gazebo)
find_package(catkin REQUIRED COMPONENTS
roscpp
gazebo_ros
)
find_package(gazebo REQUIRED)
catkin_package(
DEPENDS
roscpp
gazebo_ros
)
link_directories(${GAZEBO_LIBRARY_DIRS})
include_directories(${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})
install(DIRECTORY launch/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
)
#add_library(${PROJECT_NAME} src/simple_world_plugin.cpp)
#target_link_libraries(${catkin_LIBRARIES})

View File

@ -0,0 +1,10 @@
<launch>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find line_follower_gazebo)/worlds/line.world"/>
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="true"/>
<arg name="recording" value="false"/>
<arg name="debug" value="false"/>
</include>
</launch>

View File

@ -0,0 +1,54 @@
<?xml version="1.0"?>
<package format="2">
<name>line_follower_gazebo</name>
<version>0.0.0</version>
<description>The line_follower_gazebo package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="yikes@todo.todo">yikes</maintainer>
<license>TODO</license>
<build_depend>roscpp</build_depend>
<exec_depend>roscpp</exec_depend>
<build_depend>gazebo_ros</build_depend>
<exec_depend>gazebo_ros</exec_depend>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/line_follower_gazebo</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- more default parameters can be changed here -->
<export>
</export>
<buildtool_depend>catkin</buildtool_depend>
</package>

View File

@ -0,0 +1,12 @@
<?xml version="1.0" ?>
<sdf version="1.4">
<world name="default">
<include>
<uri>model://sun</uri>
</include>
<include>
<uri>model://path</uri>
<pose>0 0 0 0 0 0</pose>
</include>
</world>
</sdf>