add ugvs
This commit is contained in:
parent
ff432c29d4
commit
cb99f2377c
|
@ -0,0 +1,22 @@
|
|||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package catvehicle
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
2.1.1 (2018-06-21)
|
||||
------------------
|
||||
* Added tf for human cars
|
||||
* 'Matlab files' is now 'mfiles'
|
||||
* Added an m function convert a bagfile to a mat file.
|
||||
* Added a simulink block for ramp generation with ros parameters, useful for system identification
|
||||
* Removed depracted functions in gazebo cpp plugins while migrating from ROS Indigo to ROS Kinetic
|
||||
* Added tf nodes for publishing tf frames for humancars.
|
||||
* Contributors: Rahul Kumar Bhadani
|
||||
|
||||
2.1.0 (2018-06-10)
|
||||
------------------
|
||||
* Updated Car model to include current Inertial tensor of car body and coefficient of frictions to prevent car from toppling over at high speed.
|
||||
* Contributors: Rahul Kumar Bhadani
|
||||
|
||||
2.0.2 (2017-02-02)
|
||||
------------------
|
||||
* Removed worlds intended for the cvchallenge_task2 repository
|
||||
* Contributors: Jonathan Sprinkle
|
|
@ -0,0 +1,121 @@
|
|||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(catvehicle)
|
||||
SET( CMAKE_CXX_FLAGS "-std=c++11 -O3")
|
||||
## Find catkin macros and libraries
|
||||
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
|
||||
## is used, also find other catkin packages
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
controller_manager
|
||||
gazebo_ros_control
|
||||
geometry_msgs
|
||||
position_controllers
|
||||
roscpp
|
||||
rospy
|
||||
sensor_msgs
|
||||
sicktoolbox
|
||||
sicktoolbox_wrapper
|
||||
std_msgs
|
||||
tf
|
||||
transmission_interface
|
||||
velocity_controllers
|
||||
velodyne_pointcloud
|
||||
)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
find_package(Boost REQUIRED COMPONENTS system)
|
||||
|
||||
# Need to have these in order to find/include boost stuff
|
||||
include_directories(${Boost_INCLUDE_DIRS})
|
||||
link_directories(${Boost_LIBRARY_DIRS})
|
||||
|
||||
include (FindPkgConfig)
|
||||
if (PKG_CONFIG_FOUND)
|
||||
pkg_check_modules(GAZEBO gazebo)
|
||||
endif()
|
||||
include_directories(${GAZEBO_INCLUDE_DIRS}
|
||||
# ${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
link_directories(${GAZEBO_LIBRARY_DIRS})
|
||||
include_directories(include)
|
||||
|
||||
## Uncomment this if the package has a setup.py. This macro ensures
|
||||
## modules and global scripts declared therein get installed
|
||||
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
|
||||
catkin_python_setup()
|
||||
|
||||
###################################
|
||||
## catkin specific configuration ##
|
||||
###################################
|
||||
## The catkin_package macro generates cmake config files for your package
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
LIBRARIES catvehicle
|
||||
CATKIN_DEPENDS geometry_msgs roscpp rospy sensor_msgs std_msgs velodyne_pointcloud tf
|
||||
# DEPENDS system_lib
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(include include/catvehicle)
|
||||
include_directories(
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
|
||||
add_library(catvehiclegazebo SHARED src/cont.cc ./include/catvehicle/cont.hh)
|
||||
target_link_libraries(catvehiclegazebo ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
|
||||
|
||||
add_executable( distanceEstimator src/distanceEstimator.cpp )
|
||||
target_link_libraries( distanceEstimator ${catkin_LIBRARIES} )
|
||||
|
||||
add_executable( distanceEstimatorSteeringBased src/distanceEstimatorSteeringBased.cpp )
|
||||
target_link_libraries( distanceEstimatorSteeringBased ${catkin_LIBRARIES})
|
||||
|
||||
add_executable( velocityEstimator src/velocityEstimator.cpp )
|
||||
target_link_libraries( velocityEstimator ${catkin_LIBRARIES} )
|
||||
|
||||
#############
|
||||
## Install ##
|
||||
#############
|
||||
|
||||
# all install targets should use catkin DESTINATION variables
|
||||
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
|
||||
|
||||
# added for running the simulation details without need to have
|
||||
# the information locally sourced
|
||||
install(DIRECTORY launch
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
|
||||
|
||||
install(DIRECTORY meshes
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
|
||||
|
||||
install(DIRECTORY urdf
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
|
||||
|
||||
install(DIRECTORY worlds
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
|
||||
|
||||
## Mark executables and/or libraries for installation
|
||||
install(TARGETS catvehiclegazebo
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_catvehicle.cpp)
|
||||
# if(TARGET ${PROJECT_NAME}-test)
|
||||
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
|
||||
# endif()
|
||||
|
||||
## Add folders to be run by python nosetests
|
||||
# catkin_add_nosetests(test)
|
||||
|
|
@ -0,0 +1,135 @@
|
|||
# Cognitive and Autonomous Test Vehicle (CAT Vehicle) Testbed
|
||||
The CAT Vehicle is a ROS based simulator to facilitate the development of autonomous vehicle applications. This repository houses the files that utilize the Gazebo simulator, and additional interfaces to the physical CAT Vehicle Testbed available at the University of Arizona - Department of Electrical and Computer Engineering.
|
||||
|
||||
# Dependencies
|
||||
* ROS
|
||||
* [obstaclestopper](https://github.com/jmscslgroup/obstaclestopper)
|
||||
* [control_toolbox](https://github.com/jmscslgroup/control_toolbox)
|
||||
* [sicktoolbox](https://github.com/jmscslgroup/sicktoolbox)
|
||||
* [sicktoolbox_wrapper](https://github.com/jmscslgroup/sicktoolbox_wrapper)
|
||||
* [stepvel](https://github.com/jmscslgroup/stepvel)
|
||||
* [cmdvel2gazebo](https://github.com/jmscslgroup/cmdvel2gazebo)
|
||||
* Controller manager (allows the car to move around with ROS messages)
|
||||
```shell
|
||||
sudo apt-get install ros-melodic-controller-manager
|
||||
sudo apt-get install ros-melodic-ros-control ros-melodic-ros-controllers
|
||||
sudo apt-get install ros-melodic-gazebo-ros-control
|
||||
sudo apt-get install libpcap-dev
|
||||
```
|
||||
|
||||
# System Requirements
|
||||
* Ubuntu 18.04 LTS (We cannot guarantee if it works on any other version of Ubuntu)
|
||||
* RAM: 4GB required, > 8GB recommended.
|
||||
|
||||
|
||||
# Citing this work
|
||||
If you find this work useful please give credits to the authors and developers by citing:
|
||||
```json
|
||||
Rahul Bhadani, Jonathan Sprinkle, Matthew Bunting. "The CAT Vehicle Testbed:
|
||||
A Simulator with Hardware in the Loop for Autonomous Vehicle Applications".
|
||||
Proceedings 2nd International Workshop on Safe Control of Autonomous Vehicles (SCAV 2018),
|
||||
Porto, Portugal, 10th April 2018, Electronic Proceedings in Theoretical Computer Science 269,
|
||||
pp. 32–47. Download: http://dx.doi.org/10.4204/EPTCS.269.4.
|
||||
```
|
||||
|
||||
bibtex:
|
||||
```
|
||||
@article{bhadani2018cat,
|
||||
title={{The CAT Vehicle Testbed: A Simulator with Hardware
|
||||
in the Loop for Autonomous Vehicle Applications}},
|
||||
author={Bhadani, Rahul and Sprinkle, Jonathan and Bunting, Matthew},
|
||||
journal={{Proceedings of 2nd International Workshop on Safe Control of Autonomous Vehicles
|
||||
(SCAV 2018), Porto, Portugal, 10th April 2018, Electronic Proceedings
|
||||
in Theoretical Computer Science 269, pp. 32–47}},
|
||||
year={2018}
|
||||
}
|
||||
```
|
||||
|
||||
# What's new
|
||||
* Released for Ubuntu 18.04 LTS, ROS Melodic and Gazebo 9.0
|
||||
* Support for front camera
|
||||
* More stable vehicle dynamics
|
||||
* Bug fixes and improvements
|
||||
|
||||
# How to use it
|
||||
|
||||
## Installing ROS
|
||||
* Follow the steps mentioned in the [ROS wiki page](http://wiki.ros.org/melodic/Installation/Ubuntu%C2%A0) on how to install ROS Melodic.
|
||||
* In addition to that we are required to install some additional ros packages
|
||||
```shell
|
||||
sudo apt-get install ros-melodic-velodyne ros-melodic-novatel-span-driver
|
||||
```
|
||||
|
||||
## Creating catkin workspace
|
||||
In order to use the catvehicle ROS package, you should work within a catkin workspace. If you do not already have one:
|
||||
```shell
|
||||
cd ~
|
||||
mkdir -p catvehicle_ws/src
|
||||
cd catvehicle_ws/src
|
||||
catkin_init_workspace
|
||||
cd ..
|
||||
catkin_make
|
||||
```
|
||||
|
||||
At this point, you can extract this release package from [catvehicle-3.0.1](https://github.com/jmscslgroup/catvehicle/releases/download/3.0.1/catvehicle-3.0.1.tar.xz) and other dependent package into your src directory
|
||||
```shell
|
||||
cd ~/catvehicle_ws/src
|
||||
git clone https://github.com/jmscslgroup/catvehicle
|
||||
git clone https://github.com/jmscslgroup/obstaclestopper
|
||||
git clone https://github.com/jmscslgroup/control_toolbox
|
||||
git clone https://github.com/jmscslgroup/sicktoolbox
|
||||
git clone https://github.com/jmscslgroup/sicktoolbox_wrapper
|
||||
git clone https://github.com/jmscslgroup/stepvel
|
||||
git clone https://github.com/jmscslgroup/cmdvel2gazebo
|
||||
git clone https://github.com/jmscslgroup/velodyne
|
||||
cd ..
|
||||
catkin_make
|
||||
```
|
||||
## Sourcing workspace to the environment path
|
||||
```bash
|
||||
echo "source ~/catvehicle_ws/devel/setup.bash" >> ~/.bashrc
|
||||
```
|
||||
|
||||
# Simple tutorial and examples
|
||||
Follow the tutorials on the CAT Vehicle Testbed group on the [CPS Virtual Organization](https://cps-vo.org/group/CATVehicleTestbed) to see how to use the testbed.
|
||||
|
||||
# Issues
|
||||
If you run into a problem, please feel free to post to [issues](https://github.com/jmscslgroup/catvehicle/issues). If the issue is urgent, please email to catvehicle@list.arizona.edu.
|
||||
|
||||
# Acknowledgements
|
||||
## License
|
||||
Copyright (c) 2013-2020 Arizona Board of Regents; The University of Arizona
|
||||
All rights reserved
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
## Authors and contributors
|
||||
* Jonathan Sprinkle (sprinkjm@email.arizona.edu)
|
||||
* Rahul Bhadani (rahulbhadani@email.arizona.edu)
|
||||
* Sam Taylor
|
||||
* Kennon McKeever (kennondmckeever@email.arizona.edu)
|
||||
* Alex Warren
|
||||
* Swati Munjal (smunjal@email.arizona.edu)
|
||||
* Ashley Kang (askang@email.arizona.edu)
|
||||
* Matt Bunting (mosfet@email.arizona.edu)
|
||||
* Sean Whitsitt
|
||||
|
||||
## Support
|
||||
This work was supported by the National Science Foundation and AFOSR under awards 1659428, 1521617, 1446435, 1262960 and 1253334. Any opinions, findings, and conclusions or recommendations expressed in this material are those of the author(s) and do not necessarily reflect the views of the National Science Foundation.
|
||||
|
|
@ -0,0 +1,280 @@
|
|||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 0
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /Grid1
|
||||
- /Path1
|
||||
- /PointCloud1
|
||||
- /LaserScan1
|
||||
Splitter Ratio: 0.522352933883667
|
||||
Tree Height: 827
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: LaserScan
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 100
|
||||
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
|
||||
back_left_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
back_right_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
base_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
camera_left_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
camera_right_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_laser_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_left_steering_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_left_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_right_steering_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_right_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
main_mass:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
triclops_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
velodyne_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
Name: RobotModel
|
||||
Robot Description: catvehicle/robot_description
|
||||
TF Prefix: catvehicle
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 164; 0; 0
|
||||
Enabled: true
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Lines
|
||||
Line Width: 0.029999999329447746
|
||||
Name: Path
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Radius: 0.029999999329447746
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic: /catvehicle/path
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 17.596113204956055
|
||||
Min Value: 0.654435396194458
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: AxisColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: PointCloud
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.4000000059604645
|
||||
Style: Spheres
|
||||
Topic: /catvehicle/lidar_points
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: -9999
|
||||
Min Value: 9999
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/LaserScan
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: AxisColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: -999999
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 999999
|
||||
Name: LaserScan
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.5
|
||||
Style: Spheres
|
||||
Topic: /catvehicle/front_laser_points
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 211; 215; 207
|
||||
Default Light: true
|
||||
Fixed Frame: world
|
||||
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: 153.59103393554688
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 6.550182342529297
|
||||
Y: -12.366673469543457
|
||||
Z: 7.577612400054932
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 1.5697963237762451
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 3.3453927040100098
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 978
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: true
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000020c00000378fc0200000009fc0000003d00000378000000c900fffffffa000000000100000002fb000000100044006900730070006c0061007900730100000000ffffffff0000015600fffffffb0000000a0049006d00610067006500000000000000020c0000000000000000fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d0061006700650000000200000001b50000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000280fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000280000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000001c0fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005a00000003cfc0100000002fb0000000800540069006d00650000000000000005a0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000056e0000037800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: true
|
||||
Width: 1920
|
||||
X: 0
|
||||
Y: 27
|
|
@ -0,0 +1,90 @@
|
|||
# Author: Jonathan Sprinkle
|
||||
# Copyright (c) 2015 Arizona Board of Regents
|
||||
# All rights reserved.
|
||||
#
|
||||
# Permission is hereby granted, without written agreement and without
|
||||
# license or royalty fees, to use, copy, modify, and distribute this
|
||||
# software and its documentation for any purpose, provided that the
|
||||
# above copyright notice and the following two paragraphs appear in
|
||||
# all copies of this software.
|
||||
#
|
||||
# IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
# FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
# ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
# IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
# SUCH DAMAGE.
|
||||
#
|
||||
# THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
# INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
# AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
# IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
|
||||
# TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
# Summary:
|
||||
# This YAML file includes the controller connectivity for ROS-based control
|
||||
# through Gazebo. For more information and for the tutorials used to create
|
||||
# this file, see
|
||||
# http://gazebosim.org/tutorials/?tut=ros_control#Prerequisites
|
||||
|
||||
# controls the rear two tires based on individual motors
|
||||
# Publish all joint states -----------------------------------
|
||||
joint_state_controller:
|
||||
type: joint_state_controller/JointStateController
|
||||
publish_rate: 50
|
||||
|
||||
# Velocity Controllers ---------------------------------------
|
||||
|
||||
# The left/right rear wheels are what moves the car. This
|
||||
# controller set uses PID for velocity set points
|
||||
joint1_velocity_controller:
|
||||
type: velocity_controllers/JointVelocityController
|
||||
joint: back_left_wheel_joint
|
||||
pid: {p: 100.0, i: 0.01, d: 10.0}
|
||||
|
||||
joint2_velocity_controller:
|
||||
type: velocity_controllers/JointVelocityController
|
||||
joint: back_right_wheel_joint
|
||||
pid: {p: 100.0, i: 0.01, d: 10.0}
|
||||
|
||||
# we are no longer using these controllers for angular rate
|
||||
# but we may reinstate them later
|
||||
#front_left_steering_controller:
|
||||
# type: velocity_controllers/JointVelocityController
|
||||
# joint: front_left_steering_joint
|
||||
# pid: {p: 100.0, i: 0.01, d: 10.0}
|
||||
#front_right_steering_controller:
|
||||
# type: velocity_controllers/JointVelocityController
|
||||
# joint: front_right_steering_joint
|
||||
# pid: {p: 100.0, i: 0.01, d: 10.0}
|
||||
|
||||
# permits a set point for the steering (tire) angle, which
|
||||
# is enforced by these position controllers. The value is
|
||||
# set based on the dynamics of the controller, not an
|
||||
# instantaneous set for the position
|
||||
front_left_steering_position_controller:
|
||||
type: position_controllers/JointPositionController
|
||||
joint: front_left_steering_joint
|
||||
pid: {p: 100.0, i: 0.01, d: 10.0}
|
||||
front_right_steering_position_controller:
|
||||
type: position_controllers/JointPositionController
|
||||
joint: front_right_steering_joint
|
||||
pid: {p: 100.0, i: 0.01, d: 10.0}
|
||||
|
||||
#gazebo_ros_control:
|
||||
# pid_gains:
|
||||
# back_left_wheel_joint:
|
||||
# p: 100.0
|
||||
# i: 0.01
|
||||
# d: 10.0
|
||||
# back_right_wheel_joint:
|
||||
# p: 100.0
|
||||
# i: 0.01
|
||||
# d: 10.0
|
||||
# front_left_steering_joint:
|
||||
# p: 100.0
|
||||
# i: 0.01
|
||||
# d: 10.0
|
||||
# front_right_steering_joint:
|
||||
# p: 100.0
|
||||
# i: 0.01
|
||||
# d: 10.0
|
|
@ -0,0 +1,279 @@
|
|||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 0
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /Grid1
|
||||
- /RobotModel1
|
||||
- /Path1
|
||||
Splitter Ratio: 0.522352933883667
|
||||
Tree Height: 827
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: LaserScan
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 100
|
||||
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
|
||||
back_left_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
back_right_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
base_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
camera_left_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
camera_right_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_laser_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_left_steering_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_left_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_right_steering_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_right_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
main_mass:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
triclops_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
velodyne_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
Name: RobotModel
|
||||
Robot Description: magna/robot_description
|
||||
TF Prefix: magna
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 204; 0; 0
|
||||
Enabled: true
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Lines
|
||||
Line Width: 0.029999999329447746
|
||||
Name: Path
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Radius: 0.029999999329447746
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic: /magna/path
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 17.596113204956055
|
||||
Min Value: 0.654435396194458
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: AxisColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: PointCloud
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.4000000059604645
|
||||
Style: Spheres
|
||||
Topic: /magna/lidar_points
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: -9999
|
||||
Min Value: 9999
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/LaserScan
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: AxisColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: -999999
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 999999
|
||||
Name: LaserScan
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.5
|
||||
Style: Spheres
|
||||
Topic: /magna/front_laser_points
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 211; 215; 207
|
||||
Default Light: true
|
||||
Fixed Frame: world
|
||||
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: 81.78622436523438
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 8.990336418151855
|
||||
Y: 1.8101232051849365
|
||||
Z: 7.5045037269592285
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 1.410398006439209
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 2.740393877029419
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 978
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: true
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000020c00000378fc0200000009fc0000003d00000378000000c900fffffffa000000000100000002fb000000100044006900730070006c0061007900730100000000ffffffff0000015600fffffffb0000000a0049006d00610067006500000000000000020c0000000000000000fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d0061006700650000000200000001b50000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000280fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000280000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000001c0fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005a00000003cfc0100000002fb0000000800540069006d00650000000000000005a0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000056e0000037800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: true
|
||||
Width: 1920
|
||||
X: 0
|
||||
Y: 27
|
File diff suppressed because it is too large
Load Diff
|
@ -0,0 +1,6 @@
|
|||
image: testmap.png
|
||||
resolution: 0.1
|
||||
origin: [0.0, 0.0, 0.0]
|
||||
occupied_thresh: 0.65
|
||||
free_thresh: 0.196
|
||||
negate: 0
|
|
@ -0,0 +1,360 @@
|
|||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 70
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /PointCloud1
|
||||
- /LaserScan1
|
||||
- /Image1
|
||||
- /RobotModel2
|
||||
Splitter Ratio: 0.522352933883667
|
||||
Tree Height: 70
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: PointCloud
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
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
|
||||
back_left_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
back_right_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
base_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
camera_left_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
camera_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
camera_right_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_laser_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_left_steering_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_left_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_right_steering_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_right_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
main_mass:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
velodyne_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
Name: RobotModel
|
||||
Robot Description: catvehicle/robot_description
|
||||
TF Prefix: catvehicle
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
- Alpha: 1
|
||||
Buffer Length: 1
|
||||
Class: rviz/Path
|
||||
Color: 25; 255; 0
|
||||
Enabled: true
|
||||
Head Diameter: 0.30000001192092896
|
||||
Head Length: 0.20000000298023224
|
||||
Length: 0.30000001192092896
|
||||
Line Style: Lines
|
||||
Line Width: 0.029999999329447746
|
||||
Name: Path
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Pose Color: 255; 85; 255
|
||||
Pose Style: None
|
||||
Radius: 0.029999999329447746
|
||||
Shaft Diameter: 0.10000000149011612
|
||||
Shaft Length: 0.10000000149011612
|
||||
Topic: /catvehicle/path
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: 17.560396194458008
|
||||
Min Value: 0.6138875484466553
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/PointCloud
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: AxisColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: PointCloud
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.4000000059604645
|
||||
Style: Spheres
|
||||
Topic: /catvehicle/lidar_points
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Autocompute Intensity Bounds: true
|
||||
Autocompute Value Bounds:
|
||||
Max Value: -9999
|
||||
Min Value: 9999
|
||||
Value: true
|
||||
Axis: Z
|
||||
Channel Name: intensity
|
||||
Class: rviz/LaserScan
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: AxisColor
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: -999999
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 999999
|
||||
Name: LaserScan
|
||||
Position Transformer: XYZ
|
||||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.5
|
||||
Style: Spheres
|
||||
Topic: /catvehicle/front_laser_points
|
||||
Unreliable: false
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Image Topic: /catvehicle/camera_right/image_raw_right
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Image Topic: /catvehicle/camera_left/image_raw_left
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
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
|
||||
back_left_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
back_right_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
base_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_left_steering_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_left_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_right_steering_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
front_right_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
main_mass:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Name: RobotModel
|
||||
Robot Description: humancar/robot_description
|
||||
TF Prefix: humancar
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Default Light: true
|
||||
Fixed Frame: world
|
||||
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: 73.99931335449219
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 3.4520277976989746
|
||||
Y: -5.269349575042725
|
||||
Z: -1.3676940202713013
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.5753988027572632
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 1.4353983402252197
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 798
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: true
|
||||
Image:
|
||||
collapsed: false
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000020c000001f5fc0200000009fb0000000a0049006d006100670065010000010c000000f90000001600fffffffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065010000020b000000f60000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000280fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000280000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000005a0000000c9fc0100000002fb000000100044006900730070006c0061007900730100000000000005a00000015600fffffffb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005a00000003cfc0100000002fb0000000800540069006d00650000000000000005a0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000038e000001f500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: true
|
||||
Width: 1440
|
||||
X: 0
|
||||
Y: 27
|
|
@ -0,0 +1,48 @@
|
|||
#include <boost/bind.hpp>
|
||||
#include <gazebo/gazebo.hh>
|
||||
#include <gazebo/physics/physics.hh>
|
||||
#include <gazebo/common/common.hh>
|
||||
#include <cstdio>
|
||||
#include "ros/ros.h"
|
||||
#include "geometry_msgs/Twist.h"
|
||||
#include <gazebo/physics/Joint.hh>
|
||||
#include <cstdlib>
|
||||
//#include <unistd.h>
|
||||
|
||||
|
||||
namespace gazebo
|
||||
{
|
||||
class CmdVelController : public ModelPlugin
|
||||
{
|
||||
public:
|
||||
void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/);
|
||||
void OnUpdate(const common::UpdateInfo & /*_info*/);
|
||||
// void SteeringSmooth();
|
||||
private:
|
||||
void CatVehicleSimROSThread();
|
||||
void Callback(const geometry_msgs::Twist::ConstPtr& msg);
|
||||
|
||||
//ROS
|
||||
ros::Subscriber sub_;
|
||||
boost::thread ros_spinner_thread_;
|
||||
ros::NodeHandle* rosnode_;
|
||||
|
||||
//variables
|
||||
double u1; // commanded velocity
|
||||
double u2; // commanded tire angle
|
||||
double x1; // actual velocity
|
||||
double x2; // actual tire angle
|
||||
// double difference;
|
||||
// double prev_angle;
|
||||
// double cur_angle;
|
||||
|
||||
//Gazebo
|
||||
physics::JointPtr steering_joints[2];
|
||||
physics::JointPtr velocity_joints[2];
|
||||
// ros::NodeHandle* rosnode_;
|
||||
physics::JointController *steering_cont;
|
||||
physics::JointVelocityController *rearWheel_cont;
|
||||
event::ConnectionPtr updateConnection;
|
||||
physics::ModelPtr model;
|
||||
};
|
||||
}
|
|
@ -0,0 +1,65 @@
|
|||
#include <boost/bind.hpp>
|
||||
#include <gazebo/gazebo.hh>
|
||||
#include <gazebo/physics/physics.hh>
|
||||
#include <gazebo/common/common.hh>
|
||||
#include <cstdio>
|
||||
#include "ros/ros.h"
|
||||
#include "geometry_msgs/Twist.h"
|
||||
#include <gazebo/physics/Joint.hh>
|
||||
#include <cstdlib>
|
||||
//#include <unistd.h>
|
||||
#include<gazebo_msgs/ModelStates.h>
|
||||
#include <ignition/math/Vector3.hh>
|
||||
namespace gazebo
|
||||
{
|
||||
class CatSteering : public ModelPlugin
|
||||
{
|
||||
public:
|
||||
CatSteering();
|
||||
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
|
||||
//void OnUpdate(const common::UpdateInfo & _info);
|
||||
|
||||
private:
|
||||
void CatVehicleSimROSThread();
|
||||
void modelRead(const gazebo_msgs::ModelStates::ConstPtr& msg);
|
||||
|
||||
physics::PhysicsEnginePtr physicsEngine;
|
||||
//to read the name space from urdf file
|
||||
std::string robotNamespace;
|
||||
//to read the name space from urdf file
|
||||
std::string tfScope;
|
||||
//Name of the speed topic being published
|
||||
std::string speedTopic;
|
||||
// Name of the tire angle topic being published
|
||||
std::string tireTopic;
|
||||
// Name of the odometry topic being published
|
||||
std::string odomTopic;
|
||||
//ROS
|
||||
ros::Subscriber sub_;
|
||||
ros::Publisher ros_pub;
|
||||
ros::Publisher steering_pub;
|
||||
ros::Publisher odom_pub;
|
||||
boost::thread ros_spinner_thread_;
|
||||
ros::NodeHandle* rosnode_;
|
||||
|
||||
//velocity vector to fetch velocity from model entity
|
||||
ignition::math::Vector3<double> linear_vel;
|
||||
ignition::math::Vector3<double> angular_vel;
|
||||
//Gazebo
|
||||
physics::JointPtr steering_joints[2];
|
||||
physics::JointController *j_cont;
|
||||
event::ConnectionPtr updateConnection;
|
||||
|
||||
|
||||
//Pointer to the model entity
|
||||
physics::ModelPtr model;
|
||||
//Pointer to the world in which the model exists
|
||||
physics::WorldPtr world;
|
||||
|
||||
|
||||
//rate at which to update the catsteering
|
||||
double updateRate;
|
||||
//Previous time when the catsteering was updated.
|
||||
ros::Time prevUpdateTime;
|
||||
};
|
||||
}
|
|
@ -0,0 +1,43 @@
|
|||
#include <boost/bind.hpp>
|
||||
#include <gazebo/gazebo.hh>
|
||||
#include <gazebo/physics/physics.hh>
|
||||
#include <gazebo/common/common.hh>
|
||||
#include <stdio.h>
|
||||
#include "ros/ros.h"
|
||||
#include "geometry_msgs/Twist.h"
|
||||
#include <gazebo/physics/Joint.hh>
|
||||
#include <stdlib.h>
|
||||
//#include <unistd.h>
|
||||
|
||||
|
||||
namespace gazebo
|
||||
{
|
||||
class LeadSteering : public ModelPlugin
|
||||
{
|
||||
public:
|
||||
void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/);
|
||||
void OnUpdate(const common::UpdateInfo & /*_info*/);
|
||||
// void SteeringSmooth();
|
||||
private:
|
||||
void LeadVehicleSimROSThread();
|
||||
void Callback(const geometry_msgs::Twist::ConstPtr& msg);
|
||||
|
||||
//ROS
|
||||
ros::Subscriber sub_;
|
||||
boost::thread ros_spinner_thread_;
|
||||
ros::NodeHandle* rosnode_;
|
||||
|
||||
//variables
|
||||
double angle;
|
||||
// double difference;
|
||||
// double prev_angle;
|
||||
// double cur_angle;
|
||||
|
||||
//Gazebo
|
||||
physics::JointPtr steering_joints[2];
|
||||
// ros::NodeHandle* rosnode_;
|
||||
physics::JointController *j_cont;
|
||||
event::ConnectionPtr updateConnection;
|
||||
physics::ModelPtr model;
|
||||
};
|
||||
}
|
|
@ -0,0 +1,40 @@
|
|||
#include <boost/bind.hpp>
|
||||
#include <gazebo/gazebo.hh>
|
||||
#include <gazebo/physics/physics.hh>
|
||||
#include <gazebo/common/common.hh>
|
||||
#include <gazebo/math/gzmath.hh>
|
||||
#include <stdio.h>
|
||||
#include "ros/ros.h"
|
||||
#include "nav_msgs/Odometry.h"
|
||||
#include <stdlib.h>
|
||||
|
||||
|
||||
namespace gazebo
|
||||
{
|
||||
class CatGPS : public ModelPlugin
|
||||
{
|
||||
public:
|
||||
void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/);
|
||||
void OnUpdate(const common::UpdateInfo & /*_info*/);
|
||||
private:
|
||||
void CatVehicleSimROSThread();
|
||||
void Callback(const nav_msgs::Odometry::ConstPtr& msg);
|
||||
|
||||
|
||||
//ROS
|
||||
ros::Subscriber sub_;
|
||||
boost::thread ros_spinner_thread_;
|
||||
ros::NodeHandle* rosnode_;
|
||||
|
||||
//gazebo
|
||||
event::ConnectionPtr updateConnection;
|
||||
physics::ModelPtr model;
|
||||
|
||||
//variables
|
||||
int k;
|
||||
double x , y , z , x_ang , y_ang , z_ang , omega , x_new , y_new , offset_x , offset_y , car_height;
|
||||
math::Vector3 vector;
|
||||
math::Pose _pose;
|
||||
math::Quaternion quaternion;
|
||||
};
|
||||
}
|
|
@ -0,0 +1,107 @@
|
|||
<?xml version="1.0"?>
|
||||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle, Rahul Bhadani
|
||||
Copyright (c) 2016-2020 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
=======
|
||||
This launch file is used to broadcast tf and robot parameters for playback
|
||||
or live visualization of data through rviz
|
||||
|
||||
How to execute it:
|
||||
=================
|
||||
roslaunch catvehicle catvehicle-tf.launch
|
||||
|
||||
-->
|
||||
<launch>
|
||||
|
||||
<arg name="robot_name" default="catvehicle"/>
|
||||
|
||||
<arg name="tyre_height" default="0.7"/>
|
||||
<arg name="halftyre_height" default="0.32"/>
|
||||
<arg name="car_width" default="0.77"/>
|
||||
<arg name="car_length" default="1.55"/>
|
||||
<arg name="car_height" default="1.5837572084" />
|
||||
<arg name="tyre_front_x" default="1.52"/>
|
||||
<arg name="tyre_back_x" default="-1.1"/>
|
||||
|
||||
<include file="$(find catvehicle)/launch/robotviz.launch"/>
|
||||
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="odom2ins_$(arg robot_name)"
|
||||
args="-0.8 -$(arg car_width) 0.0 0 0 0 catvehicle/odom catvehicle/base_link 10"/>
|
||||
|
||||
<!-- JMS adapted this to refer to base_link rather than ins
|
||||
<node pkg="tf" type="static_transform_publisher" name="baselink2velodyne_$(arg robot_name)"
|
||||
args="1.2 0 0.75 0 0 0 catvehicle/ins velodyne 10"/>-->
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="baselink2velodyne_$(arg robot_name)"
|
||||
args="-0.6 0 2.12 0 0 0 catvehicle/base_link velodyne 10"/>
|
||||
|
||||
|
||||
<!-- convert from the velodyne to a /catvehicle/velodyne_link frame -->
|
||||
<node pkg="tf" type="static_transform_publisher" name="velodyne2velodyne_link_$(arg robot_name)"
|
||||
args="0 0 0 0 0 0 velodyne catvehicle/velodyne_link 10"/>
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="baselinkr2front_laser_link_tf_$(arg robot_name)"
|
||||
args="2.5 0.0 1.1 0.0 0 0 catvehicle/base_link catvehicle/front_laser_link 75"/>
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="baselink_laser_tf_$(arg robot_name)"
|
||||
args="2.5 0.0 1.1 0.0 0 0 catvehicle/base_link laser 75"/>
|
||||
|
||||
<!-- JMS: Note, I think that if we stop doing this, things will make more sense; should be a tf broadcasted
|
||||
from the odometrybroadcaster_node in a package w/ hwil capabilities -->
|
||||
<!--node pkg="tf" type="static_transform_publisher" name="baselink2odom_tf_$(arg robot_name)"
|
||||
args="-1.25 0 $(arg halftyre_height) 0 0 0 catvehicle/base_link catvehicle/odom 75"/-->
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="baselink2mainmass_tf_$(arg robot_name)"
|
||||
args="0 0 0 0 0 0 catvehicle/base_link catvehicle/main_mass 20"/>
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="baselink2leftFtire_tf_$(arg robot_name)"
|
||||
args="$(arg tyre_front_x) -$(arg car_width) $(arg tyre_height) 3.14159265359 0 0 catvehicle/base_link catvehicle/front_left_wheel_link 10"/>
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="baselink2rightFtire_tf_$(arg robot_name)"
|
||||
args="$(arg tyre_front_x) $(arg car_width) $(arg tyre_height) 3.14159265359 0 0 catvehicle/base_link catvehicle/front_right_wheel_link 10"/>
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="baselink2leftRtire_tf_$(arg robot_name)"
|
||||
args="$(arg tyre_back_x) -$(arg car_width) $(arg tyre_height) 3.14159265359 0 0 catvehicle/base_link catvehicle/back_left_wheel_link 10"/>
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="baselink2rightRtire_tf_$(arg robot_name)"
|
||||
args="$(arg tyre_back_x) $(arg car_width) $(arg tyre_height) 3.14159265359 0 0 catvehicle/base_link catvehicle/back_right_wheel_link 10"/>
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="rightFtire2rightFtiresteering_tf_$(arg robot_name)"
|
||||
args="0 0 0 0 0 0 catvehicle/front_right_wheel_link catvehicle/front_right_steering_link 10"/>
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="leftFtire2leftFtiresteering_tf_$(arg robot_name)"
|
||||
args="0 0 0 0 0 0 catvehicle/front_left_wheel_link catvehicle/front_left_steering_link 10"/>
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="velodynelink2cameraleft_tf_$(arg robot_name)"
|
||||
args="-0.56 0.44 0 0 0 0 catvehicle/velodyne_link catvehicle/camera_left_link 5"/>
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="velodynelink2cameraright_tf_$(arg robot_name)"
|
||||
args="-0.56 -0.44 0 0 0 0 catvehicle/velodyne_link catvehicle/camera_right_link 5"/>
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="velodynelink2camera_tf_$(arg robot_name)"
|
||||
args="0.54 0 -0.08 0 0 0 catvehicle/velodyne_link catvehicle/triclops_link 5"/>
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,106 @@
|
|||
<?xml version="1.0" ?>
|
||||
|
||||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle, Rahul Bhadani
|
||||
Copyright (c) 2015-2016 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
=======
|
||||
This launch file loads the worlds and models for the catvehicle
|
||||
|
||||
|
||||
-->
|
||||
|
||||
<launch>
|
||||
<arg name="robot_name"/>
|
||||
<arg name="init_pose"/>
|
||||
<arg name="config_file"/>
|
||||
<arg name="obstaclestopper"/>
|
||||
<rosparam param="/use_sim_time">true</rosparam>
|
||||
|
||||
<node name="urdf_spawner$(arg robot_name)" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
|
||||
args="$(arg init_pose) -urdf -model $(arg robot_name) -param robot_description"/>
|
||||
|
||||
|
||||
<!-- Load joint controller configurations from YAML file to parameter server -->
|
||||
<rosparam file="$(find catvehicle)/config/$(arg config_file)" command="load" ns="/$(arg robot_name)"/>
|
||||
<param name="tf_prefix" value="$(arg robot_name)"/>
|
||||
|
||||
<!-- load the controllers -->
|
||||
<node name="controller_spawner$(arg robot_name)" pkg="controller_manager" type="spawner" respawn="false"
|
||||
output="screen" ns="/$(arg robot_name)" args="joint1_velocity_controller joint2_velocity_controller front_left_steering_position_controller front_right_steering_position_controller joint_state_controller"/>
|
||||
|
||||
<!-- convert joint states to TF transforms for rviz, etc -->
|
||||
<node name="robot_state_publisher$(arg robot_name)" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen">
|
||||
<remap from="/joint_states" to="/$(arg robot_name)/joint_states" />
|
||||
</node>
|
||||
|
||||
|
||||
<!-- need for publishing joint states that are not controlled -->
|
||||
<node name="joint_state_publisher$(arg robot_name)" pkg="joint_state_publisher" type="joint_state_publisher" respawn="false" output="screen">
|
||||
<remap from="/joint_states" to="/$(arg robot_name)/joint_states" />
|
||||
</node>
|
||||
|
||||
<!-- set up a static TF transform for publishing SLAM localization estimates wrt base_link -->
|
||||
<node pkg="tf" type="static_transform_publisher" name="base_link2slamodom_tf_$(arg robot_name)"
|
||||
args="0 0 0 0 0 1 $(arg robot_name)/base_link $(arg robot_name)/slamodom 5" />
|
||||
|
||||
|
||||
<node name="cmdvel2gazebo$(arg robot_name)" pkg="catvehicle" type="cmdvel2gazebo.py" respawn="false" output="screen">
|
||||
<remap from="/$(arg robot_name)/cmd_vel" to="/$(arg robot_name)/cmd_vel_safe" if="$(arg obstaclestopper)"/>
|
||||
</node>
|
||||
|
||||
<!--node name="distanceEstimatorSteeringBased" pkg="catvehicle" type="distanceEstimatorSteeringBased" output="screen">
|
||||
<param name="scan_topic" value="/$(arg robot_name)/front_laser_points"/>
|
||||
<param name="steering_topic" value="/$(arg robot_name)/steering"/>
|
||||
<remap from="/$(arg robot_name)/distanceEstimatorSteeringBased/dist" to="/$(arg robot_name)/distanceEstimatorSteeringBased/dist" />
|
||||
<remap from="/$(arg robot_name)/distanceEstimatorSteeringBased/Xdist" to="/$(arg robot_name)/distanceEstimatorSteeringBased/Xdist" />
|
||||
<remap from="/$(arg robot_name)/distanceEstimatorSteeringBased/Ydist" to="/$(arg robot_name)/distanceEstimatorSteeringBased/Ydist" />
|
||||
<remap from="/$(arg robot_name)/distanceEstimatorSteeringBased/angle" to="/$(arg robot_name)/distanceEstimatorSteeringBased/angle" />
|
||||
<param name="tmin" value="-6.0"/--><!-- This is like the xmin in the cartesian windowed estimator -->
|
||||
<!--param name="tmax" value="3.0"/--><!-- This is like the xmax in the cartesian windowed estimator -->
|
||||
<!--param name="smin" value="0.0"/--><!-- This is like the ymin in the cartesian windowed estimator -->
|
||||
<!--param name="smax" value="80.0"/--><!-- This is like the ymax in the cartesian windowed estimator -->
|
||||
<!--param name="epsilon" value="0.005"/--><!-- This is the minimum turning angle for the ring transformation vs a direct cartiesian calculation (to avoid infinite turning radius in calculation) -->
|
||||
|
||||
<!--/node-->
|
||||
|
||||
<node name="distanceEstimator" pkg="catvehicle" type="distanceEstimator" output="screen">
|
||||
<param name="scan_topic" value="/$(arg robot_name)/front_laser_points"/>
|
||||
<param name="angle_min" value="-10.0"/>
|
||||
<param name="angle_max" value="10.0"/>
|
||||
|
||||
</node>
|
||||
|
||||
<!-- Let's be safe with obstacles by stopping when we get too close-->
|
||||
<node name="obstacleStopper$(arg robot_name)" pkg="obstaclestopper" type="obstaclestopper_node" output="screen" if="$(arg obstaclestopper)"/>
|
||||
|
||||
<!-- publish the path information of this vehicle -->
|
||||
<node name="odom2path$(arg robot_name)" pkg="catvehicle" type="odom2path.py" respawn="false" output="screen" args="-n /$(arg robot_name)">
|
||||
</node>
|
||||
|
||||
<!-- establish this vehicle's place in the global frame for tf transforms -->
|
||||
<node pkg="tf" type="static_transform_publisher" name="global_frame_tf_$(arg robot_name)"
|
||||
args="0 0 0 0 0 0 /world $(arg robot_name)/odom 100"/>
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,61 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle, Sam Taylor, Alex Warren
|
||||
Copyright (c) 2015 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE UNIVERSITY OF CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
This launch file loads the worlds and models for the catvehicle
|
||||
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<arg name="paused" default="false"/>
|
||||
<arg name="use_sim_time" default="true"/>
|
||||
<arg name="gui" default="false"/>
|
||||
<arg name="headless" default="false"/>
|
||||
<arg name="debug" default="false"/>
|
||||
<arg name="obstaclestopper" default="true"/>
|
||||
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="world_name" value="$(find catvehicle)/worlds/canyonview_field.world"/>
|
||||
<arg name="debug" value="$(arg debug)" />
|
||||
<arg name="gui" value="$(arg gui)" />
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
|
||||
<arg name="headless" value="$(arg headless)"/>
|
||||
</include>
|
||||
|
||||
<group ns="catvehicle">
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro.py '$(find catvehicle)/urdf/catvehicle.xacro' roboname:='catvehicle'" />
|
||||
|
||||
<include file="$(find catvehicle)/launch/catvehicle.launch">
|
||||
<arg name="robot_name" value="catvehicle"/>
|
||||
<arg name="init_pose" value="-x 1 -y 1 -z 0"/>
|
||||
<arg name="obstaclestopper" value="$(arg obstaclestopper)"/>
|
||||
<arg name="config_file" value="catvehicle_control.yaml"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,58 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!--
|
||||
|
||||
Author: Rahul Kumar Bhadani
|
||||
Copyright (c) 2018 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
=======
|
||||
This launch file loads the worlds and models for the catvehicle
|
||||
|
||||
How to execute it:
|
||||
=================
|
||||
roslaunch catvehicle catvehicle_empty.launch
|
||||
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- JMS trying to set statistics param -->
|
||||
<param name="enable_statistics" value="true" />
|
||||
|
||||
|
||||
<arg name="paused" default="false"/>
|
||||
<arg name="use_sim_time" default="true"/>
|
||||
<arg name="gui" default="true"/>
|
||||
<arg name="headless" default="false"/>
|
||||
<arg name="debug" default="false"/>
|
||||
|
||||
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="world_name" value="$(find catvehicle)/worlds/city.world"/>
|
||||
<arg name="debug" value="$(arg debug)" />
|
||||
<arg name="gui" value="$(arg gui)" />
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
|
||||
<arg name="headless" value="$(arg headless)"/>
|
||||
</include>
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,72 @@
|
|||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle, Sam Taylor, Alex Warren
|
||||
Copyright (c) 2015 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
This launch file loads the worlds and models for the catvehicle
|
||||
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<arg name="paused" default="false"/>
|
||||
<arg name="use_sim_time" default="true"/>
|
||||
<arg name="gui" default="false"/>
|
||||
<arg name="headless" default="false"/>
|
||||
<arg name="debug" default="false"/>
|
||||
<arg name="worldfile" default="enter_worldfile_no_path.world"/>
|
||||
|
||||
<!-- change these defaults here, or as a cmd line arg, in order to turn off -->
|
||||
<!-- sensors that you aren't using, thus freeing up computational resources -->
|
||||
<!-- and reducing storage space for your bagfiles -->
|
||||
<arg name="front_laser_points" default="true"/>
|
||||
<arg name="velodyne_points" default="true"/>
|
||||
<arg name="camera_right" default="true"/>
|
||||
<arg name="camera_left" default="true"/>
|
||||
<!-- change these defaults here, to see further left with the velodyne -->
|
||||
<arg name="velodyne_max_angle" default="0.4"/>
|
||||
<!-- change these defaults here, to see further right with the velodyne -->
|
||||
<arg name="velodyne_min_angle" default="-0.4"/>
|
||||
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="world_name" value="$(find catvehicle)/worlds/$(arg worldfile)"/>
|
||||
<arg name="debug" value="$(arg debug)" />
|
||||
<arg name="gui" value="$(arg gui)" />
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
|
||||
<arg name="headless" value="$(arg headless)"/>
|
||||
</include>
|
||||
|
||||
<group ns="catvehicle">
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro.py '$(find catvehicle)/urdf/catvehicle.xacro' roboname:='catvehicle' front_laser_points:='$(arg front_laser_points)' velodyne_points:='$(arg velodyne_points)' camera_right:='$(arg camera_right)' camera_left:='$(arg camera_left)' velodyne_max_angle:='$(arg velodyne_max_angle)' velodyne_min_angle:='$(arg velodyne_min_angle)'" />
|
||||
|
||||
<include file="$(find catvehicle)/launch/catvehicle.launch">
|
||||
<arg name="robot_name" value="catvehicle"/>
|
||||
<arg name="init_pose" value="-x 0 -y 0 -z 0"/>
|
||||
<arg name="config_file" value="catvehicle_control.yaml"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,56 @@
|
|||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle
|
||||
Copyright (c) 2015 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
=======
|
||||
This launch file loads the worlds and models for the catvehicle
|
||||
|
||||
How to execute it:
|
||||
=================
|
||||
roslaunch catvehicle catvehicle_empty.launch
|
||||
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- JMS trying to set statistics param -->
|
||||
<param name="enable_statistics" value="true" />
|
||||
|
||||
|
||||
<arg name="paused" default="false"/>
|
||||
<arg name="use_sim_time" default="true"/>
|
||||
<arg name="gui" default="false"/>
|
||||
<arg name="headless" default="false"/>
|
||||
<arg name="debug" default="false"/>
|
||||
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="world_name" value="$(find catvehicle)/worlds/plane.world"/>
|
||||
<arg name="debug" value="$(arg debug)" />
|
||||
<arg name="gui" value="$(arg gui)" />
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
|
||||
<arg name="headless" value="$(arg headless)"/>
|
||||
</include>
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,80 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle, Sam Taylor, Alex Warren
|
||||
Copyright (c) 2015 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
This launch file loads the worlds and models for the catvehicle
|
||||
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<arg name="paused" default="false"/>
|
||||
<arg name="use_sim_time" default="true"/>
|
||||
<arg name="gui" default="false"/>
|
||||
<arg name="headless" default="false"/>
|
||||
<arg name="debug" default="false"/>
|
||||
<arg name="obstaclestopper" default="true"/>
|
||||
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="debug" value="$(arg debug)" />
|
||||
<arg name="gui" value="$(arg gui)" />
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
|
||||
<arg name="headless" value="$(arg headless)"/>
|
||||
</include>
|
||||
|
||||
<group ns="catvehicle">
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro.py '$(find catvehicle)/urdf/catvehicle.xacro' roboname:='catvehicle'" />
|
||||
<include file="$(find catvehicle)/launch/catvehicle.launch">
|
||||
<arg name="robot_name" value="catvehicle"/>
|
||||
<arg name="init_pose" value="-x 1 -y 1 -z 0"/>
|
||||
<arg name="config_file" value="catvehicle_control.yaml"/>
|
||||
<arg name="obstaclestopper" value="$(arg obstaclestopper)"/>
|
||||
</include>
|
||||
|
||||
<!-- include information for the joystick, etc. -->
|
||||
<node pkg="joy" type="joy_node" name="joy_node" output="screen"
|
||||
required="true">
|
||||
</node>
|
||||
|
||||
<node pkg="catvehicle" type="joy2cmdvel.py" name="joy2cmdvel" output="screen"
|
||||
required="true">
|
||||
<param name="namespace" value="/catvehicle"/>
|
||||
</node>
|
||||
|
||||
|
||||
</group>
|
||||
<group ns="follower_sim">
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro.py '$(find catvehicle)/urdf/catvehicle.xacro' roboname:='follower_sim'" />
|
||||
<include file="$(find catvehicle)/launch/catvehicle.launch">
|
||||
<arg name="robot_name" value="follower_sim"/>
|
||||
<arg name="init_pose" value="-x -10 -y 1 -z 0"/>
|
||||
<arg name="config_file" value="catvehicle_control.yaml"/>
|
||||
<arg name="obstaclestopper" value="$(arg obstaclestopper)"/>
|
||||
</include>
|
||||
</group>
|
||||
</launch>
|
|
@ -0,0 +1,61 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle, Sam Taylor, Alex Warren
|
||||
Copyright (c) 2015 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
This launch file loads the worlds and models for the catvehicle
|
||||
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<arg name="paused" default="false"/>
|
||||
<arg name="use_sim_time" default="true"/>
|
||||
<arg name="gui" default="false"/>
|
||||
<arg name="headless" default="false"/>
|
||||
<arg name="debug" default="false"/>
|
||||
<arg name="obstaclestopper" default="true"/>
|
||||
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="world_name" value="$(find catvehicle)/worlds/neighborhood.world"/>
|
||||
<arg name="debug" value="$(arg debug)" />
|
||||
<arg name="gui" value="$(arg gui)" />
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
|
||||
<arg name="headless" value="$(arg headless)"/>
|
||||
</include>
|
||||
|
||||
<group ns="catvehicle">
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro.py '$(find catvehicle)/urdf/catvehicle.xacro' roboname:='catvehicle'" />
|
||||
<include file="$(find catvehicle)/launch/catvehicle.launch">
|
||||
<arg name="robot_name" value="catvehicle"/>
|
||||
<arg name="obstaclestopper" value="$(arg obstaclestopper)"/>
|
||||
<arg name="init_pose" value="-x 0 -y 0 -z 0"/>
|
||||
<arg name="config_file" value="catvehicle_control.yaml"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,70 @@
|
|||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle, Sam Taylor, Alex Warren
|
||||
Copyright (c) 2015 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
========
|
||||
|
||||
This launch file loads the worlds and models for the catvehicle with the name catvehicle in stationary state.
|
||||
|
||||
How to execute this file?
|
||||
========================
|
||||
|
||||
roslaunch catvehicle catvehicle_skidpan.launch
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<arg name="paused" default="false"/>
|
||||
<arg name="use_sim_time" default="true"/>
|
||||
<arg name="gui" default="true"/>
|
||||
<arg name="headless" default="false"/>
|
||||
<arg name="debug" default="false"/>
|
||||
<arg name="obstaclestopper" default="true"/>
|
||||
|
||||
<param name="use_sim_time" value="true"/>
|
||||
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="world_name" value="$(find catvehicle)/worlds/skidpan.world"/>
|
||||
<arg name="debug" value="$(arg debug)" />
|
||||
<arg name="gui" value="$(arg gui)" />
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
|
||||
<arg name="headless" value="$(arg headless)"/>
|
||||
</include>
|
||||
|
||||
<group ns="catvehicle">
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro.py '$(find catvehicle)/urdf/catvehicle.xacro' roboname:='catvehicle'" />
|
||||
<include file="$(find catvehicle)/launch/catvehicle.launch">
|
||||
<arg name="robot_name" value="catvehicle"/>
|
||||
<arg name="init_pose" value="-x 1 -y 1 -z 0"/>
|
||||
<arg name="config_file" value="catvehicle_control.yaml"/>
|
||||
<arg name="obstaclestopper" value="$(arg obstaclestopper)"/>
|
||||
</include>
|
||||
|
||||
<!-- Uncomment this to get immediate motion from the car
|
||||
<node name="openLoopCircle" pkg="safeopenloopcircle" type="safeopenloopcircle_node"/>
|
||||
-->
|
||||
</group>
|
||||
</launch>
|
|
@ -0,0 +1,71 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle, Sam Taylor, Alex Warren, Rahul Bhadani
|
||||
Copyright (c) 2015-2020 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE UNIVERSITY OF CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
=======
|
||||
|
||||
This launch file loads the autonomous car in stationary state into the world"
|
||||
|
||||
Prerequisite:
|
||||
=============
|
||||
1. Load the world
|
||||
roslaunch catvehicle catvehicle_empty.launch
|
||||
|
||||
How to execute this file?
|
||||
========================
|
||||
|
||||
roslaunch catvehicle catvehicle_spawn.launch robot:=acar_sim X:=0 Y:=0 Z:=0 roll:=0 pitch:=0 yaw:=0
|
||||
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- JMS trying to set statistics param -->
|
||||
<param name="enable_statistics" value="true" />
|
||||
<arg name="robot" default="catvehicle"/>
|
||||
<arg name="X" default="0"/>
|
||||
<arg name="Y" default="0"/>
|
||||
<arg name="Z" default="0"/>
|
||||
<arg name="roll" default="0"/>
|
||||
<arg name="pitch" default="0"/>
|
||||
<arg name="yaw" default="0"/>
|
||||
<arg name="camera_left" default="false"/>
|
||||
<arg name="camera_right" default="false"/>
|
||||
<arg name="triclops" default="false"/>
|
||||
<arg name="velodyne_points" default="false"/>
|
||||
<arg name="laser_sensor" default="false"/>
|
||||
<arg name="obstaclestopper" default="false"/>
|
||||
<group ns="$(arg robot)">
|
||||
<param name="robot_description" command="$(find xacro)/xacro.py '$(find catvehicle)/urdf/catvehicle.xacro' roboname:='$(arg robot)' camera_left:='$(arg camera_left)' camera_right:='$(arg camera_right)' triclops:='$(arg triclops)' velodyne_points:='$(arg velodyne_points)' laser_points:='$(arg laser_sensor)'" />
|
||||
<include file="$(find catvehicle)/launch/catvehicle.launch">
|
||||
<arg name="robot_name" value="$(arg robot)"/>
|
||||
<arg name="obstaclestopper" value="$(arg obstaclestopper)"/>
|
||||
<arg name="init_pose" value="-x $(arg X) -y $(arg Y) -z $(arg Z) -R $(arg roll) -P $(arg pitch) -Y $(arg yaw)"/>
|
||||
<arg name="config_file" value="catvehicle_control.yaml"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,66 @@
|
|||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle, Sam Taylor, Alex Warren
|
||||
Copyright (c) 2015 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE UNIVERSITY OF CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
=======
|
||||
|
||||
This launch file loads the autonomous car in stationary state into the world"
|
||||
|
||||
Prerequisite:
|
||||
=============
|
||||
1. Load the world
|
||||
roslaunch catvehicle catvehicle_empty.launch
|
||||
|
||||
How to execute this file?
|
||||
========================
|
||||
|
||||
roslaunch catvehicle catvehicle_spawn_nocmd.launch robot1:=acar_sim X:=0 Y:=0 Z:=0 roll:=0 pitch:=0 yaw:=0
|
||||
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- JMS trying to set statistics param -->
|
||||
<param name="enable_statistics" value="true" />
|
||||
<arg name="robot1" default="catvehicle"/>
|
||||
<arg name="X" default="0"/>
|
||||
<arg name="Y" default="0"/>
|
||||
<arg name="Z" default="0"/>
|
||||
<arg name="roll" default="0"/>
|
||||
<arg name="pitch" default="0"/>
|
||||
<arg name="yaw" default="0"/>
|
||||
<arg name="obstaclestopper" default="true"/>
|
||||
|
||||
<group ns="$(arg robot1)">
|
||||
<param name="robot_description" command="$(find xacro)/xacro.py '$(find catvehicle)/urdf/catvehicle.xacro' roboname:='$(arg robot1)'" />
|
||||
<include file="$(find catvehicle)/launch/catvehicle.launch">
|
||||
<arg name="robot_name" value="$(arg robot1)"/>
|
||||
<arg name="init_pose" value="-x $(arg X) -y $(arg Y) -z $(arg Z) -R $(arg roll) -P $(arg pitch) -Y $(arg yaw)"/>
|
||||
<arg name="config_file" value="catvehicle_control.yaml"/>
|
||||
<arg name="obstaclestopper" value="$(arg obstaclestopper)"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
<launch>
|
||||
|
||||
<node pkg="catvehicle_demo" type="catvehicle_demo_node" name="catvehicle_demo" output="screen">
|
||||
<param name="/linearvel" value="9.0"/>
|
||||
</node>
|
||||
</launch>
|
|
@ -0,0 +1,43 @@
|
|||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle, Sam Taylor, Alex Warren
|
||||
Copyright (c) 2015 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE UNIVERSITY OF CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
This launch file loads the worlds and models for the azcar
|
||||
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<arg name="robot" default="catvehicle"/>
|
||||
|
||||
<node pkg="catvehicle" type="distanceEstimator" name="distanceEstimator" output="screen"
|
||||
required="true">
|
||||
<param name="scan_topic" value="/scan"/>
|
||||
<param name="scan_topic" value="/$(arg robot)/scan"/>
|
||||
<param name="dist_topic" value="dist"/>
|
||||
<param name="angle_topic" value="angle"/>
|
||||
<param name="angle_min" value="-0.1"/>
|
||||
<param name="angle_max" value="0.1"/>
|
||||
</node>
|
||||
</launch>
|
|
@ -0,0 +1,42 @@
|
|||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle, Sam Taylor, Alex Warren
|
||||
Copyright (c) 2015 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE UNIVERSITY OF CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
Simulator version of the distanceEstimator
|
||||
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<arg name="scan_topic" default="/scan"/>
|
||||
|
||||
<node pkg="catvehicle" type="distanceEstimator" name="distanceEstimator" output="screen"
|
||||
required="true">
|
||||
<param name="scan_topic" value="$(arg scan_topic)"/>
|
||||
<param name="dist_topic" value="dist"/>
|
||||
<param name="angle_topic" value="angle"/>
|
||||
<param name="angle_min" value="-0.1"/>
|
||||
<param name="angle_max" value="0.1"/>
|
||||
</node>
|
||||
</launch>
|
|
@ -0,0 +1,42 @@
|
|||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle, Sam Taylor, Alex Warren
|
||||
Copyright (c) 2015 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE UNIVERSITY OF CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
This launch file runs the distanceEstimator off of the /scan topic (argument is called scan_topic)
|
||||
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<arg name="scan_topic" default="/scan"/>
|
||||
|
||||
<node pkg="catvehicle" type="distanceEstimator" name="distanceEstimator" output="screen"
|
||||
required="true">
|
||||
<param name="scan_topic" value="$(arg scan_topic)"/>
|
||||
<param name="dist_topic" value="dist"/>
|
||||
<param name="angle_topic" value="angle"/>
|
||||
<param name="angle_min" value="-0.1"/>
|
||||
<param name="angle_max" value="0.1"/>
|
||||
</node>
|
||||
</launch>
|
|
@ -0,0 +1,15 @@
|
|||
<?xml version="1.0" ?>
|
||||
<launch>
|
||||
|
||||
<arg name="robot" default="toyota"/>
|
||||
<arg name="rosbag" default="false"/>
|
||||
<arg name="csvfile" default="/home/ivory/CyverseData/JmscslgroupData/PandaData/2020_02_18/2020-02-18-13-00-42-209119__CAN_Messages.csv"/>
|
||||
<arg name="dbcfile" default="/home/ivory/VersionControl/Jmscslgroup/strym/examples/newToyotacode.dbc"/>
|
||||
<group ns="$(arg robot)">
|
||||
<!-- load the predict rospy class-->
|
||||
<node pkg="catvehicle" type="drive_lead.py" name="drive_lead_$(arg robot)" output="screen" required="true" args="$(arg csvfile) $(arg dbcfile)">
|
||||
</node>
|
||||
<node name="recorder" pkg="rosbag" type="record" output="screen" args=" -o /home/ivory/CyverseData/JmscslgroupData/Bagfiles/RNN_Prediction/CAN2ROS -a" if="$(arg rosbag)"/>
|
||||
</group>
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,109 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
|
||||
<!--
|
||||
|
||||
Author: Rahul Kumar Bhadani
|
||||
Copyright (c) 2020 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE UNIVERSITY OF CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
|
||||
-->
|
||||
|
||||
|
||||
<launch>
|
||||
|
||||
<include file="$(find catvehicle)/launch/catvehicle_empty.launch">
|
||||
|
||||
</include>
|
||||
|
||||
|
||||
<param name="enable_statistics" value="true" />
|
||||
<arg name="robot" default="catvehicle"/>
|
||||
<arg name="leader" default="toyota"/>
|
||||
<arg name="dx_min" default = "4.5"/>
|
||||
<arg name="dx_activate" default = "6.0"/>
|
||||
<arg name="velodyne_points" default="false"/>
|
||||
<arg name="camera_left" default="false"/>
|
||||
<arg name="camera_right" default="false"/>
|
||||
<arg name="triclops" default="false"/>
|
||||
<arg name="ego_laser_sensor" default="true"/>
|
||||
<arg name="leader_laser_sensor" default="true"/>
|
||||
<arg name="obstaclestopper" default="false"/>
|
||||
|
||||
<arg name="rosbag" default="false"/>
|
||||
|
||||
<arg name="csvfile" default="/home/ivory/CyverseData/JmscslgroupData/PandaData/2020_02_18/2020-02-18-13-00-42-209119__CAN_Messages.csv"/>
|
||||
<arg name="dbcfile" default="/home/ivory/VersionControl/Jmscslgroup/strym/examples/newToyotacode.dbc"/>
|
||||
|
||||
<group ns="$(arg leader)">
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro.py '$(find catvehicle)/urdf/catvehicle.xacro' roboname:='$(arg leader)' camera_left:='$(arg camera_left)' camera_right:='$(arg camera_right)' triclops:='$(arg triclops)' velodyne_points:='$(arg velodyne_points)' laser_points:='$(arg leader_laser_sensor)'" />
|
||||
<include file="$(find catvehicle)/launch/catvehicle.launch">
|
||||
<arg name="robot_name" value="$(arg leader)"/>
|
||||
<arg name="obstaclestopper" value="$(arg obstaclestopper)"/>
|
||||
<arg name="init_pose" value="-x 15.0 -y 0.0 -z 0.0"/>
|
||||
<arg name="config_file" value="catvehicle_control.yaml"/>
|
||||
</include>
|
||||
|
||||
|
||||
<node pkg="catvehicle" type="drive_lead.py" name="drive_lead_$(arg leader)" output="screen" required="true" args="$(arg csvfile) $(arg dbcfile)">
|
||||
<remap from="cmd_vel" to="cmd_control_vel"/>
|
||||
</node>
|
||||
|
||||
<node pkg="hoffmansubsystem" type="hoffmansubsystem_node" name="hoffmansubsystem_$(arg leader)" output="screen" required="true"/>
|
||||
|
||||
</group>
|
||||
|
||||
<node name="recorder" pkg="rosbag" type="record" output="screen" args=" -o /home/ivory/VersionControl/Jmscslgroup/safetyfs/bagfiles/fs-test1_dxmin_$(arg dx_min)_dx_activate_$(arg dx_activate) -a" if="$(arg rosbag)"/>
|
||||
|
||||
<group ns="$(arg robot)">
|
||||
<param name="robot_description"
|
||||
command="$(find xacro)/xacro.py '$(find catvehicle)/urdf/catvehicle.xacro' roboname:='$(arg robot)' camera_left:='$(arg camera_left)' camera_right:='$(arg camera_right)' triclops:='$(arg triclops)' velodyne_points:='$(arg velodyne_points)' laser_points:='$(arg ego_laser_sensor)'" />
|
||||
<include file="$(find catvehicle)/launch/catvehicle.launch">
|
||||
<arg name="robot_name" value="$(arg robot)"/>
|
||||
<arg name="obstaclestopper" value="$(arg obstaclestopper)"/>
|
||||
<arg name="init_pose" value="-x 0.0 -y 0.0 -z 0.0"/>
|
||||
<arg name="config_file" value="catvehicle_control.yaml"/>
|
||||
</include>
|
||||
|
||||
<!--node pkg="kf" type="kf_node" name="kf_$(arg robot)" output="screen" required="true">
|
||||
<remap from="d_relative" to="/$(arg robot)/distanceEstimatorSteeringBased/dist"/>
|
||||
</node-->
|
||||
|
||||
<!--node name="velocityEstimator" pkg="catvehicle" type="velocityEstimator" output="screen">
|
||||
<param name="dist_topic" value="/$(arg robot)/distanceEstimatorSteeringBased/dist"/>
|
||||
<param name="vel_topic" value="/$(arg robot)/v_relative"/>
|
||||
</node-->
|
||||
|
||||
<node pkg="fs" type="fs_node" name="fs_$(arg robot)" output="screen" required="true">
|
||||
<!--remap from="v_ref" to="/$(arg leader)/vel"/-->
|
||||
<remap from="d_relative" to="/$(arg robot)/distanceEstimator/dist"/>
|
||||
<remap from="cmd_vel" to="cmd_control_vel"/>
|
||||
</node>
|
||||
|
||||
<node pkg="hoffmansubsystem" type="hoffmansubsystem_node" name="hoffmansubsystem_$(arg robot)" output="screen" required="true"/>
|
||||
|
||||
</group>
|
||||
|
||||
|
||||
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,54 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
|
||||
<!--
|
||||
|
||||
Author: Rahul Kumar Bhadani
|
||||
Copyright (c) 2018 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE UNIVERSITY OF CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
|
||||
-->
|
||||
|
||||
|
||||
<launch>
|
||||
|
||||
<param name="enable_statistics" value="true" />
|
||||
<arg name="robot" default="catvehicle"/>
|
||||
<arg name="leader" default="toyota"/>
|
||||
<arg name="dx_min" default = "4.5"/>
|
||||
<arg name="dx_activate" default = "6.0"/>
|
||||
|
||||
|
||||
<group ns="$(arg robot)">
|
||||
|
||||
<node name="velocityEstimator" pkg="catvehicle" type="velocityEstimator" output="screen">
|
||||
<param name="dist_topic" value="/$(arg robot)/distanceEstimatorSteeringBased/dist"/>
|
||||
<param name="vel_topic" value="/$(arg robot)/v_relative"/>
|
||||
</node>
|
||||
|
||||
<node pkg="fs" type="fs_node" name="fs_$(arg robot)" output="screen" required="true">
|
||||
<remap from="v_ref" to="/$(arg leader)/vel"/>
|
||||
<remap from="d_relative" to="/$(arg robot)/distanceEstimatorSteeringBased/dist"/>
|
||||
</node>
|
||||
</group>
|
||||
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,77 @@
|
|||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle, Sam Taylor, Alex Warren
|
||||
Copyright (c) 2015 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
========
|
||||
|
||||
This launch file loads the SLAM algorithms using the hector_slam package
|
||||
|
||||
How to execute this file?
|
||||
========================
|
||||
|
||||
roslaunch catvehicle catvehicle_skidpan.launch
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<arg name="robot_name" default="catvehicle"/>
|
||||
|
||||
<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" respawn="false" output="screen" >
|
||||
<param name="pub_map_odom_transform" value="true"/>
|
||||
<param name="map_frame" value="map" />
|
||||
<param name="scan_topic" value="/scan" />
|
||||
<param name="base_frame" value="/laser" />
|
||||
<param name="odom_frame" value="/slamodom" />
|
||||
<param name="map_resolution" value="0.1" />
|
||||
<param name="map_size" value="500" />
|
||||
<param name="map_pub_period" value="0.5" />
|
||||
<param name="scan_subscriber_queue_size" value="100"/>
|
||||
</node>
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="map2odom_tf_$(arg robot_name)"
|
||||
args="0 0 0 0 0 0 /slamodom /laser 5"/>
|
||||
|
||||
<arg name="trajectory_source_frame_name" default="/slamodom"/>
|
||||
<arg name="trajectory_update_rate" default="4"/>
|
||||
<arg name="trajectory_publish_rate" default="0.25"/>
|
||||
|
||||
<node pkg="hector_trajectory_server" type="hector_trajectory_server" name="hector_trajectory_server" output="screen">
|
||||
<param name="target_frame_name" type="string" value="/map" />
|
||||
<param name="source_frame_name" type="string" value="$(arg trajectory_source_frame_name)" />
|
||||
<param name="trajectory_update_rate" type="double" value="$(arg trajectory_update_rate)" />
|
||||
<param name="trajectory_publish_rate" type="double" value="$(arg trajectory_publish_rate)" />
|
||||
</node>
|
||||
|
||||
|
||||
<node pkg="hector_geotiff" type="geotiff_node" name="hector_geotiff_node" output="screen" launch-prefix="nice -n 15">
|
||||
<remap from="map" to="/dynamic_map" />
|
||||
<param name="map_file_path" type="string" value="$(find catvehicle)/maps" />
|
||||
<param name="map_file_base_name" type="string" value="hector_slam_map" />
|
||||
<param name="geotiff_save_period" type="double" value="0" />
|
||||
<param name="draw_background_checkerboard" type="bool" value="true" />
|
||||
<param name="draw_free_space_grid" type="bool" value="true" />
|
||||
</node>
|
||||
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,54 @@
|
|||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle
|
||||
Copyright (c) 2015-2016 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
=======
|
||||
This launch file integrates hector slam with the car's various topics
|
||||
and frames, including the laser topic from the front of the car.
|
||||
|
||||
How to execute it:
|
||||
=================
|
||||
After starting up a simulation that includes the CAT Vehicle, then:
|
||||
|
||||
roslaunch catvehicle hectorslam.launch
|
||||
|
||||
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<arg name="robot_name" default="ugv_0"/>
|
||||
<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" respawn="false" output="screen" >
|
||||
<param name="pub_map_odom_transform" value="true"/>
|
||||
<param name="map_frame" value="map" />
|
||||
<param name="scan_topic" value="/$(arg robot_name)/front_laser_points" />
|
||||
<param name="base_frame" value="$(arg robot_name)/base_link" />
|
||||
<param name="odom_frame" value="$(arg robot_name)/odom" />
|
||||
<param name="map_resolution" value="1" />
|
||||
<param name="map_size" value="200" />
|
||||
<param name="map_pub_period" value="0.5" />
|
||||
<param name="scan_subscriber_queue_size" value="100"/>
|
||||
</node>
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,12 @@
|
|||
|
||||
<launch>
|
||||
<arg name="robot" default="catvehicle"/>
|
||||
|
||||
<node name="hoffmannsubsystem_$(arg robot)" pkg="hoffmansubsystem" type="hoffmansubsystem_node" respawn="false" output="screen">
|
||||
|
||||
<remap from="/cmd_control_vel" to="/$(arg robot)/cmd_control_vel"/>
|
||||
<remap from="/timer_companion/do_publish" to="/$(arg robot)/timer_companion/do_publish"/>
|
||||
|
||||
</node>
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,80 @@
|
|||
<?xml version="1.0" ?>
|
||||
|
||||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle, Rahul Bhadani
|
||||
Copyright (c) 2015-2020 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
=======
|
||||
This launch file loads the worlds and models for the humancars
|
||||
|
||||
-->
|
||||
|
||||
<launch>
|
||||
<arg name="robot_name"/>
|
||||
<arg name="init_pose"/>
|
||||
<arg name="config_file"/>
|
||||
|
||||
<rosparam param="/use_sim_time">true</rosparam>
|
||||
|
||||
<node name="urdf_spawner$(arg robot_name)" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
|
||||
args="$(arg init_pose) -urdf -model $(arg robot_name) -param robot_description"/>
|
||||
|
||||
|
||||
<!-- Load joint controller configurations from YAML file to parameter server -->
|
||||
<rosparam file="$(find catvehicle)/config/$(arg config_file)" command="load" ns="/$(arg robot_name)"/>
|
||||
<param name="tf_prefix" value="$(arg robot_name)"/>
|
||||
|
||||
<!-- load the controllers -->
|
||||
<node name="controller_spawner$(arg robot_name)" pkg="controller_manager" type="spawner" respawn="false"
|
||||
output="screen" ns="/$(arg robot_name)" args="joint1_velocity_controller joint2_velocity_controller front_left_steering_position_controller front_right_steering_position_controller joint_state_controller"/>
|
||||
|
||||
<!-- convert joint states to TF transforms for rviz, etc -->
|
||||
<node name="robot_state_publisher$(arg robot_name)" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen">
|
||||
<remap from="/joint_states" to="/$(arg robot_name)/joint_states" />
|
||||
</node>
|
||||
|
||||
|
||||
<!-- need for publishing joint states that are not controlled -->
|
||||
<node name="joint_state_publisher$(arg robot_name)" pkg="joint_state_publisher" type="joint_state_publisher" respawn="false" output="screen">
|
||||
<remap from="/joint_states" to="/$(arg robot_name)/joint_states" />
|
||||
</node>
|
||||
|
||||
<!-- set up a static TF transform for publishing SLAM localization estimates wrt base_link -->
|
||||
<node pkg="tf" type="static_transform_publisher" name="base_link2slamodom_tf_$(arg robot_name)"
|
||||
args="0 0 0 0 0 1 $(arg robot_name)/base_link $(arg robot_name)/slamodom 5" />
|
||||
|
||||
<!-- we run the python version, so we can be slower-than-real-time -->
|
||||
|
||||
<node name="cmdvel2gazebo$(arg robot_name)" pkg="catvehicle" type="cmdvel2gazebo.py" respawn="false" output="screen">
|
||||
</node>
|
||||
|
||||
<!-- publish the path information of this vehicle -->
|
||||
<node name="odom2path$(arg robot_name)" pkg="catvehicle" type="odom2path.py" respawn="true" output="screen" args="-n /$(arg robot_name)">
|
||||
</node>
|
||||
|
||||
<!-- establish this vehicle's place in the global frame for tf transforms -->
|
||||
<node pkg="tf" type="static_transform_publisher" name="global_frame_tf_$(arg robot_name)"
|
||||
args="0 0 0 0 0 1 /world $(arg robot_name)/odom 100"/>
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,64 @@
|
|||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle, Sam Taylor, Alex Warren, Rahul Bhadani
|
||||
Copyright (c) 2015-2020 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE UNIVERSITY OF CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
=======
|
||||
|
||||
This launch file loads the autonomous car in stationary state into the world"
|
||||
|
||||
Prerequisite:
|
||||
=============
|
||||
1. Load the world
|
||||
roslaunch humancar humancar_empty.launch
|
||||
|
||||
How to execute this file?
|
||||
========================
|
||||
|
||||
roslaunch humancar humancar_spawn.launch robot:=acar_sim X:=0 Y:=0 Z:=0 roll:=0 pitch:=0 yaw:=0
|
||||
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<!-- JMS trying to set statistics param -->
|
||||
<param name="enable_statistics" value="true" />
|
||||
<arg name="robot" default="humancar"/>
|
||||
<arg name="X" default="0"/>
|
||||
<arg name="Y" default="0"/>
|
||||
<arg name="Z" default="0"/>
|
||||
<arg name="roll" default="0"/>
|
||||
<arg name="pitch" default="0"/>
|
||||
<arg name="yaw" default="0"/>
|
||||
|
||||
<group ns="$(arg robot)">
|
||||
<param name="robot_description" command="$(find xacro)/xacro.py '$(find catvehicle)/urdf/humancar.xacro' roboname:='$(arg robot)'" />
|
||||
<include file="$(find catvehicle)/launch/humancar.launch">
|
||||
<arg name="robot_name" value="$(arg robot)"/>
|
||||
<arg name="init_pose" value="-x $(arg X) -y $(arg Y) -z $(arg Z) -R $(arg roll) -P $(arg pitch) -Y $(arg yaw)"/>
|
||||
<arg name="config_file" value="catvehicle_control.yaml"/>
|
||||
</include>
|
||||
</group>
|
||||
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,55 @@
|
|||
<?xml version="1.0" ?>
|
||||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle
|
||||
Copyright (c) 2015 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
=======
|
||||
This launch file loads the worlds and models for the catvehicle
|
||||
|
||||
How to execute it:
|
||||
=================
|
||||
roslaunch catvehicle joystick.launch
|
||||
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<arg name="robot_name" default="catvehicle" />
|
||||
<arg name="velmax" default="15.0"/>
|
||||
|
||||
<!-- include information for the joystick, etc. -->
|
||||
<node pkg="joy" type="joy_node" name="joy_node" output="screen"
|
||||
required="true">
|
||||
</node>
|
||||
|
||||
<node pkg="catvehicle" type="joy2cmdvel.py" name="joy2cmdvel" output="screen"
|
||||
required="true">
|
||||
<param name="namespace" value="/$(arg robot_name)"/>
|
||||
<param name="velmax" value="$(arg velmax)"/>
|
||||
<remap from="/catvehicle/cmd_vel" to="/$(arg robot_name)/cmd_vel" />
|
||||
</node>
|
||||
|
||||
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,13 @@
|
|||
<?xml version="1.0" ?>
|
||||
<launch>
|
||||
<arg name="rosbag" default="true"/>
|
||||
<arg name="robot" default="catvehicle"/>
|
||||
<group ns="$(arg robot)">
|
||||
<!-- load the predict rospy class-->
|
||||
<node pkg="catvehicle" type="predict.py" name="predict" output="screen" required="true">
|
||||
</node>
|
||||
<!-- load the rosbag recorder -->
|
||||
<node name="recorder" pkg="rosbag" type="record" output="screen" args=" -o /home/ivory/CyverseData/JmscslgroupData/Bagfiles/RNN_Prediction/model_prediction -a" if="$(arg rosbag)"/>
|
||||
</group>
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,54 @@
|
|||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle
|
||||
Copyright (c) 2016 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE ARIZONA BOARD OF REGENTS HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE ARIZONA BOARD OF REGENTS HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
=======
|
||||
This launch file loads the worlds and models for the azcar during playback
|
||||
or whenever gazebo is not running
|
||||
|
||||
How to execute it:
|
||||
=================
|
||||
roslaunch catvehicle robotviz.launch
|
||||
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<arg name="robot_name" default="catvehicle" />
|
||||
<arg name="front_laser_points" default="true"/>
|
||||
<arg name="velodyne_points" default="true"/>
|
||||
<arg name="camera_right" default="true"/>
|
||||
<arg name="camera_left" default="true"/>
|
||||
<!-- change these defaults here, to see further left with the velodyne -->
|
||||
<arg name="velodyne_max_angle" default="0.4"/>
|
||||
<!-- change these defaults here, to see further right with the velodyne -->
|
||||
<arg name="velodyne_min_angle" default="-0.4"/>
|
||||
|
||||
<!-- get xml file from the urdf folder -->
|
||||
<!--param name="catvehicle/robot_description" command="cat $(find catvehicle)/urdf/catvehicle_urdf.xml"/-->
|
||||
|
||||
<param name="catvehicle/robot_description"
|
||||
command="$(find xacro)/xacro.py '$(find catvehicle)/urdf/catvehicle.xacro' roboname:='catvehicle' front_laser_points:='$(arg front_laser_points)' velodyne_points:='$(arg velodyne_points)' camera_right:='$(arg camera_right)' camera_left:='$(arg camera_left)' velodyne_max_angle:='$(arg velodyne_max_angle)' velodyne_min_angle:='$(arg velodyne_min_angle)'" />
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,40 @@
|
|||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle, Sam Taylor, Alex Warren
|
||||
Copyright (c) 2015 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE UNIVERSITY OF CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
This launch file loads the worlds and models for the catvehicle in order
|
||||
to operate the SICK laser in hardware
|
||||
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<arg name="robot" default="catvehicle"/>
|
||||
|
||||
<node pkg="sicktoolbox_wrapper" type="sicklms" name="sicklms" output="screen"
|
||||
required="true">
|
||||
<param name="port" value="/dev/ttyUSB0"/>
|
||||
<param name="baud" value="500000"/>
|
||||
</node>
|
||||
</launch>
|
|
@ -0,0 +1,39 @@
|
|||
<!--
|
||||
|
||||
Author: Jonathan Sprinkle, Sam Taylor, Alex Warren
|
||||
Copyright (c) 2015 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE UNIVERSITY OF CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
This launch file tests the hardware for the sick laser
|
||||
|
||||
-->
|
||||
|
||||
<launch>
|
||||
|
||||
<include file="$(find catvehicle)/launch/sick.launch">
|
||||
</include>
|
||||
|
||||
<include file="$(find catvehicle)/launch/distanceEstimator_standalone.launch">
|
||||
</include>
|
||||
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,52 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
|
||||
<!--
|
||||
|
||||
Author: Rahul Kumar Bhadani
|
||||
Copyright (c) 2018 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE UNIVERSITY OF CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
========
|
||||
|
||||
This launch file applies a velocity profile to the leader car already loaded into the simulator.
|
||||
|
||||
|
||||
-->
|
||||
|
||||
|
||||
<launch>
|
||||
|
||||
<param name="enable_statistics" value="true" />
|
||||
<arg name="robot" default="catvehicle"/>
|
||||
<arg name="Bias" default="12.0"/> <!-- 35.5 m/s is upper limit in the velocity-->
|
||||
<arg name="Amp" default="10.0"/>
|
||||
|
||||
<param name="bias" value="$(arg Bias)"/>
|
||||
<param name="amplitude" value="$(arg Amp)"/>
|
||||
|
||||
<group ns="$(arg robot)">
|
||||
<node pkg="sinvel" type="sinvel_node" name="sinvel_$(arg robot)" output="screen" required="true"/>
|
||||
|
||||
</group>
|
||||
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,92 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
|
||||
<!--
|
||||
|
||||
Author: Rahul Kumar Bhadani
|
||||
Copyright (c) 2018 Arizona Board of Regents
|
||||
All rights reserved.
|
||||
|
||||
Permission is hereby granted, without written agreement and without
|
||||
license or royalty fees, to use, copy, modify, and distribute this
|
||||
software and its documentation for any purpose, provided that the
|
||||
above copyright notice and the following two paragraphs appear in
|
||||
all copies of this software.
|
||||
|
||||
IN NO EVENT SHALL THE ARIZONA BOARD OF REGENTS BE LIABLE TO ANY PARTY
|
||||
FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
|
||||
ARISING OUT OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN
|
||||
IF THE UNIVERSITY OF CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF
|
||||
SUCH DAMAGE.
|
||||
|
||||
THE ARIZONA BOARD OF REGENTS SPECIFICALLY DISCLAIMS ANY WARRANTIES,
|
||||
INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
|
||||
AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER
|
||||
IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION
|
||||
TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
|
||||
|
||||
Summary:
|
||||
========
|
||||
|
||||
This launch file applies a velocity profile to the leader car already loaded into the simulator.
|
||||
|
||||
Prerequisite:
|
||||
=============
|
||||
|
||||
1. Load the world
|
||||
roslaunch azcar_sim azcar_empty.launch
|
||||
|
||||
2. Load the leader
|
||||
roslaunch azcar_sim humancar_spwan_nocmd.launch robot1:=azcar_sim X:=0 Y:=0 Z:=0 roll:=0 pitch:=0 yaw:=0
|
||||
|
||||
3. Load the follower
|
||||
roslaunch azcar_sim humancar_spwan_nocmd.launch robot1:=follower_sim X:=10 Y:=0 Z:=0 roll:=0 pitch:=0 yaw:=0
|
||||
|
||||
How to execute this file?
|
||||
roslaunch azcar_sim stepvel.launch robot:=car1
|
||||
|
||||
|
||||
-->
|
||||
|
||||
|
||||
<launch>
|
||||
|
||||
<param name="enable_statistics" value="true" />
|
||||
<arg name="robot" default="catvehicle"/>
|
||||
<arg name="vel" default="2.0"/> <!-- 35.5 m/s is upper limit in the velocity-->
|
||||
<arg name="strAng" default="0.0"/>
|
||||
|
||||
<group ns="$(arg robot)">
|
||||
<param name="constVel" value="$(arg vel)"/>
|
||||
<param name="strAngle" value="$(arg strAng)"/>
|
||||
<node pkg="stepvel" type="stepvel_node" name="stepvel_$(arg robot)" output="screen" required="true"/>
|
||||
|
||||
<!--
|
||||
<node name="recorder" pkg="rosbag" type="record" output="screen" args=" -o stepvel_ -a" />
|
||||
|
||||
<node pkg="hoffmansubsystem" type="hoffmansubsystem_node" name="hoffmansubsystem_node_$(arg robot)" output="screen" required="true">
|
||||
</node>
|
||||
-->
|
||||
<!--node pkg="sin_str0_leadervel_exp4_steer" type="sin_str0_leadervel_exp4_steer_node" name="sin_str0_leadervel_exp4_steer_$(arg robot)" output="screen" required="true"-->
|
||||
<!--node pkg="azcar_sim" type="leadercmd" name="leadercmd_$(arg robot)" output="screen" required="true"-->
|
||||
<!--
|
||||
<node pkg="hoffmannfollower" type="hoffmannfollower_node" name="hoffmanfollower_$(arg robot)" output="screen" required="true">
|
||||
<remap from="/azcar_sim/vel" to="/$(arg robot)/vel"/>
|
||||
<remap from="/azcar_sim/steering" to="/$(arg robot)/steering"/>
|
||||
<remap from="/azcar_sim/cmd_vel" to="/$(arg robot)/cmd_vel"/>
|
||||
<remap from="/azcar_sim/cmd_vel_safe" to="/$(arg robot)/cmd_vel_safe"/>
|
||||
<remap from="/azcar_sim/odom" to="/$(arg robot)/odom"/>
|
||||
<remap from="/azcar_sim/waypoint" to="/$(arg robot)/waypoint"/>
|
||||
</node>
|
||||
-->
|
||||
<!--
|
||||
<node pkg="azcar_sim" type="joy2cmdvel.py" name="joy2cmdvel" output="screen"
|
||||
required="true">
|
||||
<param name="namespace" value="/$(arg robot)"/>
|
||||
<remap from="/azcar_sim/cmd_vel" to="/$(arg robot)/cmd_vel" />
|
||||
</node>
|
||||
|
||||
-->
|
||||
</group>
|
||||
|
||||
|
||||
</launch>
|
|
@ -0,0 +1,15 @@
|
|||
<?xml version="1.0" ?>
|
||||
|
||||
<launch>
|
||||
|
||||
|
||||
<arg name="robot" default="catvehicle"/>
|
||||
<arg name="dist_topic" default="/$(arg robot)/distance"/>
|
||||
<arg name="rvel_topic" default="/$(arg robot)/relative_vel"/>
|
||||
<node name="velocityEstimator" pkg="catvehicle" type="velocityEstimator" output="screen">
|
||||
<param name="dist_topic" value="$(arg dist_topic)"/>
|
||||
<param name="vel_topic" value="$(arg rvel_topic)"/>
|
||||
</node>
|
||||
|
||||
|
||||
</launch>
|
Binary file not shown.
After Width: | Height: | Size: 382 KiB |
Binary file not shown.
After Width: | Height: | Size: 426 KiB |
File diff suppressed because one or more lines are too long
Binary file not shown.
Binary file not shown.
After Width: | Height: | Size: 355 KiB |
File diff suppressed because one or more lines are too long
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
File diff suppressed because one or more lines are too long
Binary file not shown.
Binary file not shown.
File diff suppressed because one or more lines are too long
Binary file not shown.
|
@ -0,0 +1,36 @@
|
|||
# Blender MTL File: 'None'
|
||||
# Material Count: 3
|
||||
|
||||
newmtl Hybrid
|
||||
Ns 9.803922
|
||||
Ka 0.588200 0.588200 0.588200
|
||||
Kd 0.588200 0.588200 0.588200
|
||||
Ks 0.000000 0.000000 0.000000
|
||||
Ke 0.000000 0.000000 0.000000
|
||||
Ni 1.500000
|
||||
d 1.000000
|
||||
illum 2
|
||||
map_Kd Hybrid.png
|
||||
map_Ka Hybrid.png
|
||||
|
||||
newmtl Hybrid_Interior
|
||||
Ns 9.803922
|
||||
Ka 0.588200 0.588200 0.588200
|
||||
Kd 0.588200 0.588200 0.588200
|
||||
Ks 0.000000 0.000000 0.000000
|
||||
Ke 0.000000 0.000000 0.000000
|
||||
Ni 1.500000
|
||||
d 1.000000
|
||||
illum 2
|
||||
map_Kd Hybrid_Interior.png
|
||||
map_Ka Hybrid_Interior.png
|
||||
|
||||
newmtl Windows
|
||||
Ns 0.000000
|
||||
Ka 0.262700 0.262700 0.262700
|
||||
Kd 0.262700 0.262700 0.262700
|
||||
Ks 0.000000 0.000000 0.000000
|
||||
Ke 0.000000 0.000000 0.000000
|
||||
Ni 1.500000
|
||||
d 0.100000
|
||||
illum 2
|
File diff suppressed because it is too large
Load Diff
File diff suppressed because one or more lines are too long
|
@ -0,0 +1,14 @@
|
|||
# Blender MTL File: 'None'
|
||||
# Material Count: 1
|
||||
|
||||
newmtl Hybrid_Interior
|
||||
Ns 9.803922
|
||||
Ka 0.588200 0.588200 0.588200
|
||||
Kd 0.588200 0.588200 0.588200
|
||||
Ks 0.000000 0.000000 0.000000
|
||||
Ke 0.000000 0.000000 0.000000
|
||||
Ni 1.500000
|
||||
d 1.000000
|
||||
illum 2
|
||||
map_Kd Hybrid_Interior.png
|
||||
map_Ka Hybrid_Interior.png
|
|
@ -0,0 +1,572 @@
|
|||
# Blender v2.78 (sub 0) OBJ File: ''
|
||||
# www.blender.org
|
||||
mtllib steering_wheel.mtl
|
||||
o Steering_Wheel
|
||||
v 0.000000 4.525105 -16.801003
|
||||
v 6.936600 4.169304 -15.473206
|
||||
v 6.200100 1.850102 -14.253708
|
||||
v 0.000000 2.167905 -15.440109
|
||||
v 5.463501 3.248902 -12.038109
|
||||
v 0.000000 3.528904 -13.082909
|
||||
v 6.200100 5.568203 -13.257507
|
||||
v 0.000000 5.886002 -14.443810
|
||||
v 12.795601 3.156102 -11.691704
|
||||
v 11.434700 0.944802 -10.875305
|
||||
v 10.073700 2.451603 -9.062607
|
||||
v 11.434700 4.662903 -9.879005
|
||||
v 16.710503 1.639603 -6.032303
|
||||
v 14.932301 -0.409996 -5.819008
|
||||
v 13.154202 1.258404 -4.609505
|
||||
v 14.932301 3.308102 -4.822807
|
||||
v 18.085201 -0.149097 0.643394
|
||||
v 16.160500 -2.008099 0.145195
|
||||
v 14.235901 -0.149097 0.643394
|
||||
v 16.160500 1.710003 1.141495
|
||||
v 16.710400 -1.937794 7.318993
|
||||
v 14.932301 -3.606297 6.109497
|
||||
v 13.154202 -1.556599 5.896194
|
||||
v 14.932301 0.111801 7.105698
|
||||
v 12.795601 -3.454297 12.978394
|
||||
v 11.434700 -4.961094 11.165695
|
||||
v 10.073700 -2.749798 10.349297
|
||||
v 11.434700 -1.242997 12.161995
|
||||
v 6.936600 -4.467496 16.759895
|
||||
v 6.200100 -5.866299 14.544197
|
||||
v 5.463501 -3.547101 13.324791
|
||||
v 6.200100 -2.148198 15.540497
|
||||
v 0.000000 -4.823300 18.087692
|
||||
v 0.000000 -6.184201 15.730591
|
||||
v 0.000000 -3.827000 14.369591
|
||||
v 0.000000 -2.466100 16.726791
|
||||
v 5.250103 6.149003 -13.050804
|
||||
v 0.000000 6.396302 -13.973404
|
||||
v 0.000000 3.184902 -1.988502
|
||||
v 7.102703 3.213802 -2.096206
|
||||
v 5.250103 1.091103 -14.406006
|
||||
v 7.610302 -2.074596 -2.591507
|
||||
v 0.000000 -2.103397 -2.483803
|
||||
v 0.000000 1.338303 -15.328705
|
||||
v 7.250603 1.215801 5.360291
|
||||
v 8.023502 -4.229397 5.450394
|
||||
v 0.000000 0.999103 6.169197
|
||||
v 0.000000 -4.446095 6.259293
|
||||
v 12.521702 0.556602 3.655991
|
||||
v 15.570602 0.300903 4.610291
|
||||
v 16.320301 1.317703 0.815697
|
||||
v 12.521702 1.808002 -1.014107
|
||||
v 15.570602 2.019501 -1.803703
|
||||
v 16.320301 -1.578297 0.039696
|
||||
v 15.570602 -2.750897 4.415794
|
||||
v 12.521702 -2.444096 3.270798
|
||||
v 12.521702 -0.905395 -2.471909
|
||||
v 15.570602 -0.604698 -3.594109
|
||||
v 5.057003 0.435203 4.109192
|
||||
v 5.057003 -2.565594 3.723991
|
||||
v 5.057003 1.922104 -1.440109
|
||||
v 5.057003 -0.791199 -2.897903
|
||||
v -6.200098 1.850102 -14.253708
|
||||
v -6.936598 4.169304 -15.473206
|
||||
v -5.463499 3.248902 -12.038109
|
||||
v -6.200098 5.568203 -13.257507
|
||||
v -11.434698 0.944802 -10.875305
|
||||
v -12.795599 3.156102 -11.691704
|
||||
v -10.073799 2.451603 -9.062607
|
||||
v -11.434698 4.662903 -9.879005
|
||||
v -14.932299 -0.409996 -5.819008
|
||||
v -16.710499 1.639603 -6.032303
|
||||
v -13.154200 1.258404 -4.609505
|
||||
v -14.932299 3.308102 -4.822807
|
||||
v -16.160500 -2.008099 0.145195
|
||||
v -18.085199 -0.149097 0.643394
|
||||
v -14.235899 -0.149097 0.643394
|
||||
v -16.160500 1.710003 1.141495
|
||||
v -14.932299 -3.606297 6.109497
|
||||
v -16.710499 -1.937794 7.318993
|
||||
v -13.154200 -1.556599 5.896194
|
||||
v -14.932299 0.111801 7.105698
|
||||
v -11.434698 -4.961094 11.165695
|
||||
v -12.795599 -3.454297 12.978394
|
||||
v -10.073799 -2.749798 10.349297
|
||||
v -11.434698 -1.242997 12.161995
|
||||
v -6.200098 -5.866299 14.544197
|
||||
v -6.936598 -4.467496 16.759895
|
||||
v -5.463499 -3.547101 13.324791
|
||||
v -6.200098 -2.148198 15.540497
|
||||
v -5.250099 6.149003 -13.050804
|
||||
v -7.102699 3.213802 -2.096206
|
||||
v -5.250099 1.091103 -14.406006
|
||||
v -7.610298 -2.074596 -2.591507
|
||||
v -7.250599 1.215801 5.360291
|
||||
v -8.023499 -4.229397 5.450394
|
||||
v -16.320299 1.317703 0.815697
|
||||
v -15.570599 0.300903 4.610291
|
||||
v -12.521698 0.556602 3.655991
|
||||
v -12.521698 1.808002 -1.014107
|
||||
v -15.570599 2.019501 -1.803703
|
||||
v -12.521698 -2.444096 3.270798
|
||||
v -15.570599 -2.750897 4.415794
|
||||
v -16.320299 -1.578297 0.039696
|
||||
v -12.521698 -0.905395 -2.471909
|
||||
v -15.570599 -0.604698 -3.594109
|
||||
v -5.056999 0.435203 4.109192
|
||||
v -5.056999 -2.565594 3.723991
|
||||
v -5.056999 1.922104 -1.440109
|
||||
v -5.056999 -0.791199 -2.897903
|
||||
vt 0.1802 0.9952
|
||||
vt 0.1854 0.9716
|
||||
vt 0.1937 0.9752
|
||||
vt 0.1893 0.9952
|
||||
vt 0.2016 0.9787
|
||||
vt 0.1979 0.9952
|
||||
vt 0.3053 0.9101
|
||||
vt 0.3088 0.9266
|
||||
vt 0.3010 0.9301
|
||||
vt 0.2968 0.9101
|
||||
vt 0.2927 0.9337
|
||||
vt 0.2877 0.9101
|
||||
vt 0.1994 0.9518
|
||||
vt 0.2057 0.9584
|
||||
vt 0.2117 0.9648
|
||||
vt 0.3188 0.9405
|
||||
vt 0.3129 0.9468
|
||||
vt 0.3066 0.9535
|
||||
vt 0.2199 0.9388
|
||||
vt 0.2233 0.9473
|
||||
vt 0.2266 0.9555
|
||||
vt 0.3336 0.9497
|
||||
vt 0.3304 0.9580
|
||||
vt 0.3270 0.9665
|
||||
vt 0.2439 0.9342
|
||||
vt 0.2439 0.9434
|
||||
vt 0.2439 0.9523
|
||||
vt 0.3510 0.9530
|
||||
vt 0.3510 0.9619
|
||||
vt 0.3510 0.9711
|
||||
vt 0.2679 0.9388
|
||||
vt 0.2645 0.9474
|
||||
vt 0.2613 0.9556
|
||||
vt 0.3683 0.9498
|
||||
vt 0.3716 0.9580
|
||||
vt 0.3750 0.9666
|
||||
vt 0.2883 0.9519
|
||||
vt 0.2821 0.9585
|
||||
vt 0.2761 0.9648
|
||||
vt 0.3832 0.9405
|
||||
vt 0.3892 0.9469
|
||||
vt 0.3955 0.9535
|
||||
vt 0.3022 0.9716
|
||||
vt 0.2939 0.9753
|
||||
vt 0.2861 0.9787
|
||||
vt 0.3933 0.9266
|
||||
vt 0.4012 0.9301
|
||||
vt 0.4095 0.9338
|
||||
vt 0.3072 0.9952
|
||||
vt 0.2981 0.9952
|
||||
vt 0.2896 0.9952
|
||||
vt 0.3970 0.9101
|
||||
vt 0.4056 0.9101
|
||||
vt 0.4147 0.9101
|
||||
vt 0.2072 0.3898
|
||||
vt 0.2272 0.3873
|
||||
vt 0.2272 0.4338
|
||||
vt 0.2004 0.4334
|
||||
vt 0.0843 0.9239
|
||||
vt 0.0920 0.9640
|
||||
vt 0.0672 0.9643
|
||||
vt 0.0672 0.9208
|
||||
vt 0.2059 0.3688
|
||||
vt 0.2272 0.3675
|
||||
vt 0.1992 0.4632
|
||||
vt 0.1780 0.4680
|
||||
vt 0.1800 0.4350
|
||||
vt 0.2272 0.4659
|
||||
vt 0.2272 0.4870
|
||||
vt 0.1952 0.4847
|
||||
vt 0.0672 0.9940
|
||||
vt 0.0934 0.9912
|
||||
vt 0.1869 0.3868
|
||||
vt 0.2095 0.6876
|
||||
vt 0.1962 0.6902
|
||||
vt 0.1942 0.6742
|
||||
vt 0.2104 0.6667
|
||||
vt 0.1975 0.6636
|
||||
vt 0.1668 0.9432
|
||||
vt 0.1643 0.9581
|
||||
vt 0.1543 0.9542
|
||||
vt 0.1543 0.9347
|
||||
vt 0.1643 0.9309
|
||||
vt 0.1987 0.6509
|
||||
vt 0.2114 0.6537
|
||||
vt 0.2410 0.6905
|
||||
vt 0.2401 0.7028
|
||||
vt 0.2095 0.7003
|
||||
vt 0.2420 0.6665
|
||||
vt 0.1299 0.9333
|
||||
vt 0.1299 0.9557
|
||||
vt 0.2422 0.6538
|
||||
vt 0.1969 0.7020
|
||||
vt 0.1937 0.9752
|
||||
vt 0.1854 0.9716
|
||||
vt 0.2016 0.9787
|
||||
vt 0.3010 0.9301
|
||||
vt 0.3088 0.9266
|
||||
vt 0.2927 0.9337
|
||||
vt 0.2057 0.9584
|
||||
vt 0.1994 0.9518
|
||||
vt 0.2117 0.9648
|
||||
vt 0.3129 0.9468
|
||||
vt 0.3188 0.9405
|
||||
vt 0.3066 0.9535
|
||||
vt 0.2233 0.9473
|
||||
vt 0.2199 0.9388
|
||||
vt 0.2266 0.9555
|
||||
vt 0.3304 0.9580
|
||||
vt 0.3336 0.9497
|
||||
vt 0.3270 0.9665
|
||||
vt 0.2439 0.9434
|
||||
vt 0.2439 0.9342
|
||||
vt 0.2439 0.9523
|
||||
vt 0.3510 0.9619
|
||||
vt 0.3510 0.9530
|
||||
vt 0.3510 0.9711
|
||||
vt 0.2645 0.9474
|
||||
vt 0.2679 0.9388
|
||||
vt 0.2613 0.9556
|
||||
vt 0.3716 0.9580
|
||||
vt 0.3683 0.9498
|
||||
vt 0.3750 0.9666
|
||||
vt 0.2821 0.9585
|
||||
vt 0.2883 0.9519
|
||||
vt 0.2761 0.9648
|
||||
vt 0.3892 0.9469
|
||||
vt 0.3832 0.9405
|
||||
vt 0.3955 0.9535
|
||||
vt 0.2939 0.9753
|
||||
vt 0.3022 0.9716
|
||||
vt 0.2861 0.9787
|
||||
vt 0.4012 0.9301
|
||||
vt 0.3933 0.9266
|
||||
vt 0.4095 0.9338
|
||||
vt 0.2072 0.3898
|
||||
vt 0.2004 0.4334
|
||||
vt 0.0843 0.9239
|
||||
vt 0.0920 0.9640
|
||||
vt 0.2059 0.3688
|
||||
vt 0.1992 0.4632
|
||||
vt 0.1800 0.4350
|
||||
vt 0.1780 0.4680
|
||||
vt 0.1952 0.4847
|
||||
vt 0.0934 0.9912
|
||||
vt 0.1869 0.3868
|
||||
vt 0.1942 0.6742
|
||||
vt 0.1962 0.6902
|
||||
vt 0.2095 0.6876
|
||||
vt 0.2104 0.6667
|
||||
vt 0.1975 0.6636
|
||||
vt 0.1543 0.9542
|
||||
vt 0.1643 0.9581
|
||||
vt 0.1668 0.9432
|
||||
vt 0.1543 0.9347
|
||||
vt 0.1643 0.9309
|
||||
vt 0.2114 0.6537
|
||||
vt 0.1987 0.6509
|
||||
vt 0.2410 0.6905
|
||||
vt 0.2095 0.7003
|
||||
vt 0.2401 0.7028
|
||||
vt 0.2420 0.6665
|
||||
vt 0.1299 0.9333
|
||||
vt 0.1299 0.9557
|
||||
vt 0.2422 0.6538
|
||||
vt 0.1969 0.7020
|
||||
vn -0.0000 0.5000 -0.8660
|
||||
vn 0.3824 0.4620 -0.8002
|
||||
vn 0.0336 -0.8220 -0.5684
|
||||
vn -0.0000 -0.8190 -0.5738
|
||||
vn -0.3823 -0.4620 0.8003
|
||||
vn 0.0000 -0.5000 0.8660
|
||||
vn 0.0336 0.9033 0.4277
|
||||
vn 0.0000 0.9064 0.4224
|
||||
vn 0.7071 0.3536 -0.6124
|
||||
vn 0.0621 -0.8317 -0.5518
|
||||
vn -0.7071 -0.3536 0.6124
|
||||
vn 0.0621 0.8937 0.4443
|
||||
vn 0.9239 0.1913 -0.3314
|
||||
vn 0.0811 -0.8459 -0.5272
|
||||
vn -0.9239 -0.1913 0.3314
|
||||
vn 0.0811 0.8795 0.4690
|
||||
vn 1.0000 -0.0000 0.0000
|
||||
vn 0.0878 -0.8627 -0.4981
|
||||
vn -1.0000 -0.0000 -0.0000
|
||||
vn 0.0878 0.8627 0.4981
|
||||
vn 0.9239 -0.1913 0.3314
|
||||
vn 0.0811 -0.8795 -0.4690
|
||||
vn -0.9239 0.1913 -0.3314
|
||||
vn 0.0811 0.8459 0.5272
|
||||
vn 0.7071 -0.3536 0.6124
|
||||
vn 0.0621 -0.8937 -0.4443
|
||||
vn -0.7071 0.3536 -0.6124
|
||||
vn 0.0621 0.8317 0.5518
|
||||
vn 0.3824 -0.4620 0.8002
|
||||
vn 0.0336 -0.9033 -0.4277
|
||||
vn -0.3823 0.4620 -0.8003
|
||||
vn 0.0336 0.8220 0.5685
|
||||
vn 0.0000 -0.9064 -0.4224
|
||||
vn -0.0000 0.8190 0.5738
|
||||
vn 0.5721 0.7984 -0.1879
|
||||
vn -0.0000 0.9510 -0.3092
|
||||
vn -0.0000 0.8660 0.5000
|
||||
vn 0.6941 0.6584 0.2911
|
||||
vn -0.0000 -0.8660 -0.5000
|
||||
vn 0.7107 0.3608 -0.6040
|
||||
vn -0.0001 0.5000 -0.8660
|
||||
vn 0.5813 0.3696 0.7249
|
||||
vn 0.6994 -0.0756 0.7107
|
||||
vn 0.9910 0.1270 -0.0434
|
||||
vn -0.0000 0.3590 0.9333
|
||||
vn -0.0000 -0.2372 0.9715
|
||||
vn -0.0829 0.3921 0.9162
|
||||
vn -0.2080 0.3180 0.9250
|
||||
vn -0.0704 0.9945 -0.0777
|
||||
vn -0.1546 0.9831 -0.0981
|
||||
vn -0.2966 0.6913 -0.6589
|
||||
vn -0.1527 0.6771 -0.7199
|
||||
vn 0.0422 0.3044 0.9516
|
||||
vn 0.0622 -0.3789 0.9233
|
||||
vn -0.1549 -0.3751 0.9140
|
||||
vn 0.0375 0.9859 -0.1632
|
||||
vn 0.0555 0.6841 -0.7273
|
||||
vn -0.3304 -0.3289 0.8847
|
||||
vn -0.0336 -0.8220 -0.5684
|
||||
vn -0.3824 0.4620 -0.8002
|
||||
vn 0.3823 -0.4620 0.8003
|
||||
vn -0.0336 0.9033 0.4277
|
||||
vn -0.0621 -0.8317 -0.5518
|
||||
vn -0.0621 0.8937 0.4443
|
||||
vn -0.0811 -0.8459 -0.5272
|
||||
vn -0.0811 0.8795 0.4690
|
||||
vn -0.0878 -0.8627 -0.4981
|
||||
vn -0.0878 0.8627 0.4981
|
||||
vn -0.0811 -0.8795 -0.4690
|
||||
vn -0.0811 0.8459 0.5272
|
||||
vn -0.0621 -0.8937 -0.4443
|
||||
vn -0.0621 0.8317 0.5518
|
||||
vn -0.0336 -0.9033 -0.4277
|
||||
vn -0.3824 -0.4620 0.8002
|
||||
vn 0.3823 0.4620 -0.8003
|
||||
vn -0.0336 0.8220 0.5685
|
||||
vn -0.5721 0.7984 -0.1879
|
||||
vn -0.6941 0.6584 0.2911
|
||||
vn -0.7107 0.3608 -0.6040
|
||||
vn -0.5813 0.3696 0.7249
|
||||
vn -0.9910 0.1270 -0.0434
|
||||
vn -0.6994 -0.0756 0.7107
|
||||
vn 0.2080 0.3180 0.9250
|
||||
vn 0.0829 0.3921 0.9162
|
||||
vn 0.0704 0.9945 -0.0777
|
||||
vn 0.1546 0.9831 -0.0981
|
||||
vn 0.1527 0.6771 -0.7199
|
||||
vn 0.2966 0.6913 -0.6589
|
||||
vn -0.0422 0.3044 0.9516
|
||||
vn 0.1549 -0.3751 0.9140
|
||||
vn -0.0622 -0.3789 0.9233
|
||||
vn -0.0375 0.9859 -0.1632
|
||||
vn -0.0574 0.6840 -0.7272
|
||||
vn 0.3304 -0.3289 0.8847
|
||||
usemtl Hybrid_Interior
|
||||
s 1
|
||||
f 1/1/1 2/2/2 3/3/3
|
||||
f 3/3/3 4/4/4 1/1/1
|
||||
f 4/4/4 3/3/3 5/5/5
|
||||
f 5/5/5 6/6/6 4/4/4
|
||||
f 6/7/6 5/8/5 7/9/7
|
||||
f 7/9/7 8/10/8 6/7/6
|
||||
f 8/10/8 7/9/7 2/11/2
|
||||
f 2/11/2 1/12/1 8/10/8
|
||||
f 2/2/2 9/13/9 10/14/10
|
||||
f 10/14/10 3/3/3 2/2/2
|
||||
f 3/3/3 10/14/10 11/15/11
|
||||
f 11/15/11 5/5/5 3/3/3
|
||||
f 5/8/5 11/16/11 12/17/12
|
||||
f 12/17/12 7/9/7 5/8/5
|
||||
f 7/9/7 12/17/12 9/18/9
|
||||
f 9/18/9 2/11/2 7/9/7
|
||||
f 9/13/9 13/19/13 14/20/14
|
||||
f 14/20/14 10/14/10 9/13/9
|
||||
f 10/14/10 14/20/14 15/21/15
|
||||
f 15/21/15 11/15/11 10/14/10
|
||||
f 11/16/11 15/22/15 16/23/16
|
||||
f 16/23/16 12/17/12 11/16/11
|
||||
f 12/17/12 16/23/16 13/24/13
|
||||
f 13/24/13 9/18/9 12/17/12
|
||||
f 13/19/13 17/25/17 18/26/18
|
||||
f 18/26/18 14/20/14 13/19/13
|
||||
f 14/20/14 18/26/18 19/27/19
|
||||
f 19/27/19 15/21/15 14/20/14
|
||||
f 15/22/15 19/28/19 20/29/20
|
||||
f 20/29/20 16/23/16 15/22/15
|
||||
f 16/23/16 20/29/20 17/30/17
|
||||
f 17/30/17 13/24/13 16/23/16
|
||||
f 17/25/17 21/31/21 22/32/22
|
||||
f 22/32/22 18/26/18 17/25/17
|
||||
f 18/26/18 22/32/22 23/33/23
|
||||
f 23/33/23 19/27/19 18/26/18
|
||||
f 19/28/19 23/34/23 24/35/24
|
||||
f 24/35/24 20/29/20 19/28/19
|
||||
f 20/29/20 24/35/24 21/36/21
|
||||
f 21/36/21 17/30/17 20/29/20
|
||||
f 21/31/21 25/37/25 26/38/26
|
||||
f 26/38/26 22/32/22 21/31/21
|
||||
f 22/32/22 26/38/26 27/39/27
|
||||
f 27/39/27 23/33/23 22/32/22
|
||||
f 23/34/23 27/40/27 28/41/28
|
||||
f 28/41/28 24/35/24 23/34/23
|
||||
f 24/35/24 28/41/28 25/42/25
|
||||
f 25/42/25 21/36/21 24/35/24
|
||||
f 25/37/25 29/43/29 30/44/30
|
||||
f 30/44/30 26/38/26 25/37/25
|
||||
f 26/38/26 30/44/30 31/45/31
|
||||
f 31/45/31 27/39/27 26/38/26
|
||||
f 27/40/27 31/46/31 32/47/32
|
||||
f 32/47/32 28/41/28 27/40/27
|
||||
f 28/41/28 32/47/32 29/48/29
|
||||
f 29/48/29 25/42/25 28/41/28
|
||||
f 29/43/29 33/49/6 34/50/33
|
||||
f 34/50/33 30/44/30 29/43/29
|
||||
f 30/44/30 34/50/33 35/51/1
|
||||
f 35/51/1 31/45/31 30/44/30
|
||||
f 31/46/31 35/52/1 36/53/34
|
||||
f 36/53/34 32/47/32 31/46/31
|
||||
f 32/47/32 36/53/34 33/54/6
|
||||
f 33/54/6 29/48/29 32/47/32
|
||||
f 37/55/35 38/56/36 39/57/37
|
||||
f 39/57/37 40/58/38 37/55/35
|
||||
f 41/59/39 42/60/39 43/61/39
|
||||
f 43/61/39 44/62/39 41/59/39
|
||||
f 38/56/36 37/55/35 41/63/40
|
||||
f 41/63/40 44/64/41 38/56/36
|
||||
f 45/65/42 46/66/43 42/67/44
|
||||
f 42/67/44 40/58/38 45/65/42
|
||||
f 45/65/42 47/68/45 48/69/46
|
||||
f 48/69/46 46/70/43 45/65/42
|
||||
f 45/65/42 40/58/38 39/57/37
|
||||
f 39/57/37 47/68/45 45/65/42
|
||||
f 48/71/39 43/61/39 42/60/39
|
||||
f 42/60/39 46/72/39 48/71/39
|
||||
f 40/58/38 42/67/44 41/73/40
|
||||
f 41/73/40 37/55/35 40/58/38
|
||||
f 49/74/47 50/75/48 51/76/37
|
||||
f 52/77/49 49/74/47 51/76/37
|
||||
f 53/78/50 52/77/49 51/76/37
|
||||
f 54/79/39 55/80/39 56/81/39
|
||||
f 54/79/39 56/81/39 57/82/39
|
||||
f 58/83/39 54/79/39 57/82/39
|
||||
f 52/77/49 53/78/50 58/84/51
|
||||
f 58/84/51 57/85/52 52/77/49
|
||||
f 59/86/53 60/87/54 56/88/55
|
||||
f 56/88/55 49/74/47 59/86/53
|
||||
f 49/74/47 52/77/49 61/89/56
|
||||
f 61/89/56 59/86/53 49/74/47
|
||||
f 62/90/39 57/82/39 56/81/39
|
||||
f 56/81/39 60/91/39 62/90/39
|
||||
f 61/89/56 52/77/49 57/85/52
|
||||
f 57/85/52 62/92/57 61/89/56
|
||||
f 49/74/47 56/88/55 55/93/58
|
||||
f 55/93/58 50/75/48 49/74/47
|
||||
f 1/1/1 4/4/4 63/94/59
|
||||
f 63/94/59 64/95/60 1/1/1
|
||||
f 4/4/4 6/6/6 65/96/61
|
||||
f 65/96/61 63/94/59 4/4/4
|
||||
f 6/7/6 8/10/8 66/97/62
|
||||
f 66/97/62 65/98/61 6/7/6
|
||||
f 8/10/8 1/12/1 64/99/60
|
||||
f 64/99/60 66/97/62 8/10/8
|
||||
f 64/95/60 63/94/59 67/100/63
|
||||
f 67/100/63 68/101/27 64/95/60
|
||||
f 63/94/59 65/96/61 69/102/25
|
||||
f 69/102/25 67/100/63 63/94/59
|
||||
f 65/98/61 66/97/62 70/103/64
|
||||
f 70/103/64 69/104/25 65/98/61
|
||||
f 66/97/62 64/99/60 68/105/27
|
||||
f 68/105/27 70/103/64 66/97/62
|
||||
f 68/101/27 67/100/63 71/106/65
|
||||
f 71/106/65 72/107/23 68/101/27
|
||||
f 67/100/63 69/102/25 73/108/21
|
||||
f 73/108/21 71/106/65 67/100/63
|
||||
f 69/104/25 70/103/64 74/109/66
|
||||
f 74/109/66 73/110/21 69/104/25
|
||||
f 70/103/64 68/105/27 72/111/23
|
||||
f 72/111/23 74/109/66 70/103/64
|
||||
f 72/107/23 71/106/65 75/112/67
|
||||
f 75/112/67 76/113/19 72/107/23
|
||||
f 71/106/65 73/108/21 77/114/17
|
||||
f 77/114/17 75/112/67 71/106/65
|
||||
f 73/110/21 74/109/66 78/115/68
|
||||
f 78/115/68 77/116/17 73/110/21
|
||||
f 74/109/66 72/111/23 76/117/19
|
||||
f 76/117/19 78/115/68 74/109/66
|
||||
f 76/113/19 75/112/67 79/118/69
|
||||
f 79/118/69 80/119/15 76/113/19
|
||||
f 75/112/67 77/114/17 81/120/13
|
||||
f 81/120/13 79/118/69 75/112/67
|
||||
f 77/116/17 78/115/68 82/121/70
|
||||
f 82/121/70 81/122/13 77/116/17
|
||||
f 78/115/68 76/117/19 80/123/15
|
||||
f 80/123/15 82/121/70 78/115/68
|
||||
f 80/119/15 79/118/69 83/124/71
|
||||
f 83/124/71 84/125/11 80/119/15
|
||||
f 79/118/69 81/120/13 85/126/9
|
||||
f 85/126/9 83/124/71 79/118/69
|
||||
f 81/122/13 82/121/70 86/127/72
|
||||
f 86/127/72 85/128/9 81/122/13
|
||||
f 82/121/70 80/123/15 84/129/11
|
||||
f 84/129/11 86/127/72 82/121/70
|
||||
f 84/125/11 83/124/71 87/130/73
|
||||
f 87/130/73 88/131/74 84/125/11
|
||||
f 83/124/71 85/126/9 89/132/75
|
||||
f 89/132/75 87/130/73 83/124/71
|
||||
f 85/128/9 86/127/72 90/133/76
|
||||
f 90/133/76 89/134/75 85/128/9
|
||||
f 86/127/72 84/129/11 88/135/74
|
||||
f 88/135/74 90/133/76 86/127/72
|
||||
f 88/131/74 87/130/73 34/50/33
|
||||
f 34/50/33 33/49/6 88/131/74
|
||||
f 87/130/73 89/132/75 35/51/1
|
||||
f 35/51/1 34/50/33 87/130/73
|
||||
f 89/134/75 90/133/76 36/53/34
|
||||
f 36/53/34 35/52/1 89/134/75
|
||||
f 90/133/76 88/135/74 33/54/6
|
||||
f 33/54/6 36/53/34 90/133/76
|
||||
f 91/136/77 92/137/78 39/57/37
|
||||
f 39/57/37 38/56/36 91/136/77
|
||||
f 93/138/39 44/62/39 43/61/39
|
||||
f 43/61/39 94/139/39 93/138/39
|
||||
f 38/56/36 44/64/41 93/140/79
|
||||
f 93/140/79 91/136/77 38/56/36
|
||||
f 95/141/80 92/137/78 94/142/81
|
||||
f 94/142/81 96/143/82 95/141/80
|
||||
f 95/141/80 96/144/82 48/69/46
|
||||
f 48/69/46 47/68/45 95/141/80
|
||||
f 95/141/80 47/68/45 39/57/37
|
||||
f 39/57/37 92/137/78 95/141/80
|
||||
f 48/71/39 96/145/39 94/139/39
|
||||
f 94/139/39 43/61/39 48/71/39
|
||||
f 92/137/78 91/136/77 93/146/79
|
||||
f 93/146/79 94/142/81 92/137/78
|
||||
f 97/147/37 98/148/83 99/149/84
|
||||
f 97/147/37 99/149/84 100/150/85
|
||||
f 101/151/86 97/147/37 100/150/85
|
||||
f 102/152/39 103/153/39 104/154/39
|
||||
f 105/155/39 102/152/39 104/154/39
|
||||
f 106/156/39 105/155/39 104/154/39
|
||||
f 100/150/85 105/157/87 106/158/88
|
||||
f 106/158/88 101/151/86 100/150/85
|
||||
f 107/159/89 99/149/84 102/160/90
|
||||
f 102/160/90 108/161/91 107/159/89
|
||||
f 99/149/84 107/159/89 109/162/92
|
||||
f 109/162/92 100/150/85 99/149/84
|
||||
f 110/163/39 108/164/39 102/152/39
|
||||
f 102/152/39 105/155/39 110/163/39
|
||||
f 109/162/92 110/165/93 105/157/87
|
||||
f 105/157/87 100/150/85 109/162/92
|
||||
f 99/149/84 98/148/83 103/166/94
|
||||
f 103/166/94 102/160/90 99/149/84
|
|
@ -0,0 +1,14 @@
|
|||
# Blender MTL File: 'None'
|
||||
# Material Count: 1
|
||||
|
||||
newmtl Wheels3
|
||||
Ns 9.803922
|
||||
Ka 0.588200 0.588200 0.588200
|
||||
Kd 0.588200 0.588200 0.588200
|
||||
Ks 0.000000 0.000000 0.000000
|
||||
Ke 0.000000 0.000000 0.000000
|
||||
Ni 1.500000
|
||||
d 1.000000
|
||||
illum 2
|
||||
map_Kd Wheels3.png
|
||||
map_Ka Wheels3.png
|
|
@ -0,0 +1,247 @@
|
|||
# Blender v2.78 (sub 0) OBJ File: ''
|
||||
# www.blender.org
|
||||
mtllib wheel.mtl
|
||||
o Wheel_Front_Left_
|
||||
v -13.473793 -30.469116 -0.000059
|
||||
v -13.473793 -28.977814 9.415441
|
||||
v -13.473793 -24.650009 17.909340
|
||||
v -13.473793 -17.909319 24.650042
|
||||
v -13.473793 -9.415513 28.977842
|
||||
v -13.473793 -0.000017 30.469141
|
||||
v -13.473793 9.415481 28.977842
|
||||
v -13.473793 17.909384 24.650042
|
||||
v -13.473793 24.650085 17.909344
|
||||
v -13.473694 28.977890 9.415442
|
||||
v -13.473694 30.469185 -0.000056
|
||||
v -13.473694 28.977890 -9.415557
|
||||
v -13.473793 24.650085 -17.909355
|
||||
v -13.473793 17.909388 -24.650057
|
||||
v -13.473694 9.415483 -28.977957
|
||||
v -13.473793 -0.000014 -30.469158
|
||||
v -13.473793 -9.415511 -28.977858
|
||||
v -13.473793 -17.909315 -24.650057
|
||||
v -13.473793 -24.650116 -17.909258
|
||||
v -13.473793 -28.977905 -9.415458
|
||||
v 13.473709 -30.469208 0.000041
|
||||
v 13.473709 -28.977905 9.415544
|
||||
v 13.473709 -24.650009 17.909340
|
||||
v 13.473709 -17.909410 24.650042
|
||||
v 13.473709 -9.415513 28.977942
|
||||
v 13.473709 -0.000017 30.469141
|
||||
v 13.473709 9.415481 28.977942
|
||||
v 13.473709 17.909285 24.650042
|
||||
v 13.473808 24.650085 17.909344
|
||||
v 13.473808 28.977890 9.415442
|
||||
v 13.473808 30.469185 0.000043
|
||||
v 13.473808 28.977890 -9.415456
|
||||
v 13.473808 24.650085 -17.909355
|
||||
v 13.473709 17.909288 -24.650057
|
||||
v 13.473808 9.415483 -28.977858
|
||||
v 13.473808 -0.000014 -30.469158
|
||||
v 13.473709 -9.415511 -28.977858
|
||||
v 13.473709 -17.909315 -24.650057
|
||||
v 13.473709 -24.650116 -17.909258
|
||||
v 13.473709 -28.977905 -9.415458
|
||||
vt 0.9895 0.7464
|
||||
vt 0.9777 0.8224
|
||||
vt 0.9435 0.8909
|
||||
vt 0.8902 0.9452
|
||||
vt 0.8230 0.9801
|
||||
vt 0.7485 0.9921
|
||||
vt 0.6740 0.9801
|
||||
vt 0.6069 0.9452
|
||||
vt 0.5535 0.8909
|
||||
vt 0.5193 0.8224
|
||||
vt 0.5075 0.7464
|
||||
vt 0.5193 0.6705
|
||||
vt 0.5535 0.6020
|
||||
vt 0.6069 0.5477
|
||||
vt 0.6740 0.5128
|
||||
vt 0.7485 0.5007
|
||||
vt 0.8230 0.5128
|
||||
vt 0.8902 0.5477
|
||||
vt 0.9435 0.6020
|
||||
vt 0.9777 0.6705
|
||||
vt 0.4935 0.0038
|
||||
vt 0.0043 0.0038
|
||||
vt 0.0043 0.2577
|
||||
vt 0.4935 0.2577
|
||||
vt 0.4935 0.0038
|
||||
vt 0.0043 0.0038
|
||||
vt 0.0043 0.2577
|
||||
vt 0.4935 0.2577
|
||||
vt 0.0043 0.4985
|
||||
vt 0.4935 0.4985
|
||||
vt 0.4935 0.2577
|
||||
vt 0.0043 0.2577
|
||||
vt 0.0043 0.4985
|
||||
vt 0.4935 0.4985
|
||||
vt 0.4935 0.0038
|
||||
vt 0.0043 0.0038
|
||||
vt 0.0043 0.2577
|
||||
vt 0.4935 0.2577
|
||||
vt 0.0043 0.4985
|
||||
vt 0.4935 0.4985
|
||||
vt 0.4935 0.0038
|
||||
vt 0.0043 0.0038
|
||||
vt 0.0043 0.2577
|
||||
vt 0.4935 0.2577
|
||||
vt 0.0043 0.4985
|
||||
vt 0.4935 0.4985
|
||||
vt 0.4935 0.0038
|
||||
vt 0.0043 0.0038
|
||||
vt 0.0043 0.2577
|
||||
vt 0.4935 0.2577
|
||||
vt 0.0043 0.4985
|
||||
vt 0.4935 0.4985
|
||||
vt 0.4935 0.0038
|
||||
vt 0.0043 0.0038
|
||||
vt 0.0043 0.2577
|
||||
vt 0.4935 0.2577
|
||||
vt 0.0043 0.4985
|
||||
vt 0.4935 0.4985
|
||||
vt 0.4935 0.0038
|
||||
vt 0.0043 0.0038
|
||||
vt 0.0043 0.2577
|
||||
vt 0.4935 0.2577
|
||||
vt 0.0043 0.4985
|
||||
vt 0.4935 0.4985
|
||||
vt 0.4935 0.0038
|
||||
vt 0.0043 0.0038
|
||||
vt 0.0043 0.2577
|
||||
vt 0.4935 0.2577
|
||||
vt 0.0043 0.4985
|
||||
vt 0.4935 0.4985
|
||||
vt 0.4935 0.0038
|
||||
vt 0.0043 0.0038
|
||||
vt 0.0043 0.2577
|
||||
vt 0.4935 0.2577
|
||||
vt 0.0043 0.4985
|
||||
vt 0.4935 0.4985
|
||||
vt 0.4935 0.0038
|
||||
vt 0.0043 0.0038
|
||||
vt 0.0043 0.2577
|
||||
vt 0.4935 0.2577
|
||||
vt 0.0043 0.4985
|
||||
vt 0.4935 0.4985
|
||||
vt 0.9439 0.6167
|
||||
vt 0.8910 0.5640
|
||||
vt 0.8242 0.5301
|
||||
vt 0.7503 0.5185
|
||||
vt 0.6763 0.5301
|
||||
vt 0.6095 0.5640
|
||||
vt 0.5566 0.6167
|
||||
vt 0.5226 0.6831
|
||||
vt 0.5108 0.7567
|
||||
vt 0.5226 0.8304
|
||||
vt 0.5566 0.8968
|
||||
vt 0.6095 0.9495
|
||||
vt 0.6763 0.9833
|
||||
vt 0.7503 0.9950
|
||||
vt 0.8242 0.9833
|
||||
vt 0.8910 0.9495
|
||||
vt 0.9439 0.8968
|
||||
vt 0.9779 0.8304
|
||||
vt 0.9897 0.7567
|
||||
vt 0.9779 0.6831
|
||||
vn -1.0000 -0.0000 0.0000
|
||||
vn -0.0000 -1.0000 0.0000
|
||||
vn -0.0000 -0.9511 0.3090
|
||||
vn -0.0000 -0.8090 0.5878
|
||||
vn -0.0000 -0.5878 0.8090
|
||||
vn -0.0000 -0.3090 0.9511
|
||||
vn -0.0000 -0.0000 1.0000
|
||||
vn -0.0000 0.3090 0.9511
|
||||
vn 0.0000 0.5878 0.8090
|
||||
vn 0.0000 0.8090 0.5878
|
||||
vn 0.0000 0.9511 0.3090
|
||||
vn 0.0000 1.0000 0.0000
|
||||
vn 0.0000 0.9511 -0.3090
|
||||
vn 0.0000 0.8090 -0.5878
|
||||
vn 0.0000 0.5878 -0.8090
|
||||
vn 0.0000 0.3090 -0.9511
|
||||
vn 0.0000 -0.0000 -1.0000
|
||||
vn 0.0000 -0.3090 -0.9511
|
||||
vn 0.0000 -0.5878 -0.8090
|
||||
vn 0.0000 -0.8090 -0.5878
|
||||
vn 0.0000 -0.9511 -0.3090
|
||||
vn 1.0000 -0.0000 -0.0000
|
||||
vn 1.0000 0.0000 -0.0001
|
||||
usemtl Wheels3
|
||||
s 1
|
||||
f 1/1/1 2/2/1 3/3/1
|
||||
f 3/3/1 4/4/1 5/5/1
|
||||
f 1/1/1 3/3/1 5/5/1
|
||||
f 5/5/1 6/6/1 7/7/1
|
||||
f 7/7/1 8/8/1 9/9/1
|
||||
f 5/5/1 7/7/1 9/9/1
|
||||
f 1/1/1 5/5/1 9/9/1
|
||||
f 9/9/1 10/10/1 11/11/1
|
||||
f 11/11/1 12/12/1 13/13/1
|
||||
f 9/9/1 11/11/1 13/13/1
|
||||
f 1/1/1 9/9/1 13/13/1
|
||||
f 13/13/1 14/14/1 15/15/1
|
||||
f 15/15/1 16/16/1 17/17/1
|
||||
f 13/13/1 15/15/1 17/17/1
|
||||
f 1/1/1 13/13/1 17/17/1
|
||||
f 17/17/1 18/18/1 19/19/1
|
||||
f 1/1/1 17/17/1 19/19/1
|
||||
f 20/20/1 1/1/1 19/19/1
|
||||
f 1/21/2 21/22/2 22/23/3
|
||||
f 22/23/3 2/24/3 1/21/2
|
||||
f 2/25/3 22/26/3 23/27/4
|
||||
f 23/27/4 3/28/4 2/25/3
|
||||
f 3/28/4 23/27/4 24/29/5
|
||||
f 24/29/5 4/30/5 3/28/4
|
||||
f 4/31/5 24/32/5 25/33/6
|
||||
f 25/33/6 5/34/6 4/31/5
|
||||
f 5/35/6 25/36/6 26/37/7
|
||||
f 26/37/7 6/38/7 5/35/6
|
||||
f 6/38/7 26/37/7 27/39/8
|
||||
f 27/39/8 7/40/8 6/38/7
|
||||
f 7/41/8 27/42/8 28/43/9
|
||||
f 28/43/9 8/44/9 7/41/8
|
||||
f 8/44/9 28/43/9 29/45/10
|
||||
f 29/45/10 9/46/10 8/44/9
|
||||
f 9/47/10 29/48/10 30/49/11
|
||||
f 30/49/11 10/50/11 9/47/10
|
||||
f 10/50/11 30/49/11 31/51/12
|
||||
f 31/51/12 11/52/12 10/50/11
|
||||
f 11/53/12 31/54/12 32/55/13
|
||||
f 32/55/13 12/56/13 11/53/12
|
||||
f 12/56/13 32/55/13 33/57/14
|
||||
f 33/57/14 13/58/14 12/56/13
|
||||
f 13/59/14 33/60/14 34/61/15
|
||||
f 34/61/15 14/62/15 13/59/14
|
||||
f 14/62/15 34/61/15 35/63/16
|
||||
f 35/63/16 15/64/16 14/62/15
|
||||
f 15/65/16 35/66/16 36/67/17
|
||||
f 36/67/17 16/68/17 15/65/16
|
||||
f 16/68/17 36/67/17 37/69/18
|
||||
f 37/69/18 17/70/18 16/68/17
|
||||
f 17/71/18 37/72/18 38/73/19
|
||||
f 38/73/19 18/74/19 17/71/18
|
||||
f 18/74/19 38/73/19 39/75/20
|
||||
f 39/75/20 19/76/20 18/74/19
|
||||
f 19/77/20 39/78/20 40/79/21
|
||||
f 40/79/21 20/80/21 19/77/20
|
||||
f 20/80/21 40/79/21 21/81/2
|
||||
f 21/81/2 1/82/2 20/80/21
|
||||
f 39/83/22 38/84/22 37/85/22
|
||||
f 37/85/22 36/86/22 35/87/22
|
||||
f 39/83/22 37/85/22 35/87/22
|
||||
f 35/87/22 34/88/23 33/89/22
|
||||
f 33/89/22 32/90/22 31/91/22
|
||||
f 35/87/22 33/89/22 31/91/22
|
||||
f 39/83/22 35/87/22 31/91/22
|
||||
f 31/91/22 30/92/22 29/93/22
|
||||
f 29/93/22 28/94/22 27/95/22
|
||||
f 31/91/22 29/93/22 27/95/22
|
||||
f 39/83/22 31/91/22 27/95/22
|
||||
f 27/95/22 26/96/22 25/97/22
|
||||
f 25/97/22 24/98/22 23/99/22
|
||||
f 27/95/22 25/97/22 23/99/22
|
||||
f 39/83/22 27/95/22 23/99/22
|
||||
f 23/99/22 22/100/22 21/101/22
|
||||
f 39/83/22 23/99/22 21/101/22
|
||||
f 40/102/22 39/83/22 21/101/22
|
|
@ -0,0 +1,28 @@
|
|||
% Put the directory lisiting here where Rosbag Files are stored
|
||||
% This file reads rosbag file that has topic named /DistanceEstimator/dist
|
||||
% and convert them to .mat file
|
||||
% Developer: Rahul Kumar Bhadani
|
||||
% rahulbhadani@email.arizona.edu
|
||||
|
||||
DIRPath = './';
|
||||
Files=dir(DIRPath);
|
||||
cd(DIRPath);
|
||||
for k=1:length(Files)
|
||||
FileNames=Files(k).name;
|
||||
|
||||
if ((strcmp(FileNames,'.') == 0 || strcmp(FileNames,'..') == 0 ) && length(FileNames) > 3)
|
||||
if strcmp(FileNames(end-2:end),'bag')
|
||||
bag = rosbag(FileNames);
|
||||
bagselect1 = select(bag, 'Topic','/DistanceEstimator/dist');
|
||||
msgs = readMessages(bagselect1);
|
||||
Distance = zeros(1,length(msgs));
|
||||
for i = 1:length(msgs)
|
||||
Distance(i) = msgs{i}.Data;
|
||||
end
|
||||
ts = timeseries(bagselect1, 'Data');
|
||||
matFile = FileNames(1:end-4);
|
||||
matFile = strcat(matFile,'.mat');
|
||||
save(matFile,'ts','Distance');
|
||||
end
|
||||
end
|
||||
end
|
|
@ -0,0 +1,95 @@
|
|||
rosshutdown;
|
||||
rosinit('http://ivory.local:11311')
|
||||
alpha = 1.5;
|
||||
delpha = -1.5;
|
||||
|
||||
format long;
|
||||
|
||||
cmd_velPub = rospublisher('/catvehicle/cmd_vel', 'geometry_msgs/Twist');
|
||||
pause(3); % Wait to ensure publisher is registered
|
||||
|
||||
velmsg = rosmessage(cmd_velPub);
|
||||
velmsg.Angular.Z = 0;
|
||||
|
||||
velmsg.Linear.X = 0;
|
||||
|
||||
%% State 1: Accelerate to 55mph~24m/s
|
||||
accelerate(cmd_velPub, velmsg, velmsg.Linear.X , 24.0, alpha);
|
||||
fprintf('\nState 1 finished. Current velocity is %16.8f',velmsg.Linear.X);
|
||||
|
||||
%% State 2: Hold to 55mps~24m/s for 60.0 seconds
|
||||
hold_velocity(cmd_velPub, velmsg, 60.0);
|
||||
fprintf('\nState 2 finished. Current velocity is %16.8f',velmsg.Linear.X);
|
||||
|
||||
%% State 3: Decelerate to 35mph~15m/s
|
||||
decelerate(cmd_velPub, velmsg, velmsg.Linear.X , 15.0, delpha);
|
||||
fprintf('\nState 3 finished. Current velocity is %16.8f',velmsg.Linear.X);
|
||||
|
||||
%% State 4: Hold to 35mps~15m/s for 60.0 seconds
|
||||
hold_velocity(cmd_velPub, velmsg, 60.0);
|
||||
fprintf('\nState 4 finished. Current velocity is %16.8f',velmsg.Linear.X);
|
||||
|
||||
%% State 5: Accelerate to 55mph~24m/s
|
||||
accelerate(cmd_velPub, velmsg, velmsg.Linear.X , 24.0, alpha);
|
||||
fprintf('\nState 5 finished. Current velocity is %16.8f',velmsg.Linear.X);
|
||||
|
||||
%% State 6: Hold to 55mps~24m/s for 60.0 seconds
|
||||
hold_velocity(cmd_velPub, velmsg, 60.0);
|
||||
fprintf('\nState 6 finished. Current velocity is %16.8f',velmsg.Linear.X);
|
||||
|
||||
%% State 7: Decelerate to 45mph~20m/s
|
||||
decelerate(cmd_velPub, velmsg, velmsg.Linear.X , 20.0, delpha);
|
||||
fprintf('\nState 7 finished. Current velocity is %16.8f',velmsg.Linear.X);
|
||||
|
||||
%% State 8: Hold to 45mps~20m/s for 60.0 seconds
|
||||
hold_velocity(cmd_velPub, velmsg, 60.0)
|
||||
fprintf('\nState 8 finished. Current velocity is %16.8f',velmsg.Linear.X);
|
||||
|
||||
%% State 9: Accelerate to 55mph~24m/s
|
||||
accelerate(cmd_velPub, velmsg, velmsg.Linear.X , 24.0, alpha);
|
||||
fprintf('\nState 9 finished. Current velocity is %16.8f',velmsg.Linear.X);
|
||||
|
||||
%% State 10: Hold to 55mps~24m/s for 60.0 seconds
|
||||
hold_velocity(cmd_velPub, velmsg, 60.0);
|
||||
fprintf('\nState 10 finished. Current velocity is %16.8f',velmsg.Linear.X);
|
||||
|
||||
%% State 11: Decelerate to 0m/s
|
||||
decelerate(cmd_velPub, velmsg, velmsg.Linear.X , 0.0, delpha);
|
||||
fprintf('\nState 11 finished. Current velocity is %16.8f',velmsg.Linear.X);
|
||||
|
||||
function accelerate(publisher, message, vel_init, vel_end, acceleration)
|
||||
t0 = rostime('now');
|
||||
sec_t0 = double(t0.Sec)+double(t0.Nsec)*10^-9;
|
||||
while(message.Linear.X <= vel_end)
|
||||
current = rostime('now');
|
||||
sec_current = double(current.Sec)+double(current.Nsec)*10^-9;
|
||||
message.Linear.X = vel_init + acceleration*(sec_current - sec_t0);
|
||||
send(publisher,message);
|
||||
end
|
||||
end
|
||||
|
||||
function decelerate(publisher, message, vel_init, vel_end, deceleration)
|
||||
t0 = rostime('now');
|
||||
sec_t0 = double(t0.Sec)+double(t0.Nsec)*10^-9;
|
||||
while(message.Linear.X >= vel_end)
|
||||
current = rostime('now');
|
||||
sec_current = double(current.Sec)+double(current.Nsec)*10^-9;
|
||||
message.Linear.X = vel_init + deceleration*(sec_current - sec_t0);
|
||||
send(publisher,message);
|
||||
end
|
||||
end
|
||||
|
||||
function hold_velocity(publisher, message, seconds)
|
||||
t0 = rostime('now');
|
||||
sec_t0 = double(t0.Sec)+double(t0.Nsec)*10^-9;
|
||||
|
||||
while(1)
|
||||
send(publisher, message);
|
||||
current = rostime('now');
|
||||
sec_current = double(current.Sec)+double(current.Nsec)*10^-9;
|
||||
fprintf('\nDiff time: %16.8f\n',sec_current - sec_t0);
|
||||
if(sec_current - sec_t0 >= seconds)
|
||||
break;
|
||||
end
|
||||
end
|
||||
end
|
|
@ -0,0 +1,84 @@
|
|||
%Implementation of follower algorithm
|
||||
|
||||
%Developed by Rahul Kumar Bhadani <rahulbhadani@email.arizona.edu>
|
||||
|
||||
%ROS_IP = IP Address of ROS Master
|
||||
%lead = name of the model of leader AV Car
|
||||
%follower = name of the model of follower car
|
||||
|
||||
function follower_profile(ROS_IP, lead, follower)
|
||||
|
||||
%If number of argument is not two, flag message and exit.
|
||||
if nargin < 2
|
||||
sprintf('Usage: velocityProfiler(192.168.0.32, catvehicle)');
|
||||
return;
|
||||
end
|
||||
|
||||
rosshutdown;
|
||||
close all;
|
||||
modelname1 = strcat('/',lead);
|
||||
modelname2 = strcat('/',follower);
|
||||
%Connect to ROS master
|
||||
master_uri= strcat('http://',ROS_IP);
|
||||
master_uri = strcat(master_uri,':11311');
|
||||
rosinit(master_uri);
|
||||
|
||||
%get handle for cmd_vel topic for publishing the data
|
||||
velpub1 = rospublisher(strcat(modelname1,'/cmd_vel'),rostype.geometry_msgs_Twist);
|
||||
velpub2 = rospublisher(strcat(modelname2,'/cmd_vel'),rostype.geometry_msgs_Twist);
|
||||
|
||||
%get handle for speed topic for subscribing to the data
|
||||
speedsub1 = rossubscriber(strcat(modelname1,'/vel'));
|
||||
speedsub2 = rossubscriber(strcat(modelname2,'/vel'));
|
||||
|
||||
%get handle for /DistanceEstimator
|
||||
distanceEstimaterSub = rossubscriber('/DistanceEstimator/dist');
|
||||
|
||||
%Discretize timestamp
|
||||
t = 0:0.05:150;
|
||||
v1 = 3;
|
||||
v2 = 6;
|
||||
v3 = 0;
|
||||
|
||||
%Velocity profile
|
||||
input = v1.*(t<50) + v2.*(t>=50).*(t<100) + v3.*(t>= 100);
|
||||
|
||||
%Velocity profile will be sine
|
||||
%input = abs(2*sin(t));
|
||||
|
||||
%Variable to store output velocity
|
||||
output1 = zeros(length(t),1);
|
||||
output2 = zeros(length(t),1);
|
||||
|
||||
%handle for rosmessage object for velpub topic
|
||||
velMsgs1 = rosmessage(velpub1);
|
||||
velMsgs2 = rosmessage(velpub2);
|
||||
for i=1:length(t)
|
||||
velMsgs1.Linear.X = input(i);
|
||||
velMsgs1.Angular.Z = 0.0;
|
||||
%Publish on the topic /catvehicle/cmd_vel
|
||||
send(velpub1, velMsgs1);
|
||||
%Read from the topic /catvehicle/speed
|
||||
speedata1 = receive(speedsub1,10);
|
||||
distance = receive(distanceEstimaterSub,10);
|
||||
x = distance.Data;
|
||||
|
||||
%Follower control rule
|
||||
velMsgs2.Linear.X = (1/30.*x + 2/3).*speedata1.Linear.X;
|
||||
velMsgs2.Angular.Z = 0.0;
|
||||
send(velpub2, velMsgs2);
|
||||
speedata2 = receive(speedsub2,10);
|
||||
output1(i) = speedata1.Linear.X;
|
||||
output2(i) = speedata2.Linear.X;
|
||||
end
|
||||
|
||||
%Plot the input and output velocity profile
|
||||
[n, p] = size(output1);
|
||||
T = 1:n;
|
||||
plot(T, input');
|
||||
hold on;
|
||||
plot(T, output1);
|
||||
plot(T, output2);
|
||||
title('Original Data');
|
||||
legend('Input function', 'Output response of lead','Output response of follower');
|
||||
grid on;
|
|
@ -0,0 +1,65 @@
|
|||
% Author: Jonathan Sprinkle
|
||||
% Copyright (c) 2016 Arizona Board of Regents
|
||||
% See copyright file
|
||||
|
||||
% load the bagfile
|
||||
bagfile=rosbag('catvehicle_tutorial.bag');
|
||||
% select only the /catvehicle/odom topic
|
||||
odomBag=select(bagfile,'Topic','/catvehicle/odom');
|
||||
% extract timeseries data (this will take some time)
|
||||
odom = timeseries(odomBag,'Pose.Pose.Position.X','Pose.Pose.Position.Y');
|
||||
|
||||
figure
|
||||
% plot x,y vs. time
|
||||
plot(odom.Time,odom.Data(:,1),odom.Time,odom.Data(:,2))
|
||||
% legend information
|
||||
legend({'X position','Y position'})
|
||||
|
||||
%% Select the commanded input value
|
||||
% where are data interesting? Let's look at the cmd_vel_safe inputs
|
||||
cmd_vel_safeBag=select(bagfile,'Topic','/catvehicle/cmd_vel_safe');
|
||||
% this will take some time
|
||||
cmd_vel_safe=timeseries(cmd_vel_safeBag,'Linear.X','Angular.Z');
|
||||
|
||||
%% Plot raw cmd_vel_safe data
|
||||
% let's plot these data too
|
||||
figure
|
||||
% plot x,y vs. time
|
||||
plot(cmd_vel_safe.Time,cmd_vel_safe.Data(:,1),cmd_vel_safe.Time,cmd_vel_safe.Data(:,2))
|
||||
% legend information
|
||||
legend({'Commanded velocity','Commanded steering'})
|
||||
|
||||
%% Find where interesting data start appearing
|
||||
veldiffs = diff(cmd_vel_safe.Data(:,1));
|
||||
vindices = find(veldiffs);
|
||||
cmd_vel_safe.Data(veldiffs(1)-5:veldiffs(1)+5,1)
|
||||
deltadiffs = diff(cmd_vel_safe.Data(:,2));
|
||||
dindices=find(deltadiffs);
|
||||
cmd_vel_safe.Data(indices(1)-10:indices(1)+10,2)
|
||||
|
||||
disp('Note that the values within two time series will not necessarily');
|
||||
disp(' match up wrt one another');
|
||||
cmd_vel_safe.Time(vindices(1))
|
||||
odom.Time(vindices(1))
|
||||
|
||||
%% find the index nearest to a certain time
|
||||
interestingTime=cmd_vel_safe.Time(vindices(1));
|
||||
odomTmp = odom.Time > interestingTime;
|
||||
odomIndex = max(0,find(odomTmp)-5); % back up 5 indices, if they exist;
|
||||
|
||||
|
||||
%% now, we plot side-by-side from our relative indices
|
||||
figure
|
||||
plot(odom.Time(odomIndex(1):end),odom.Data(odomIndex(1):end,1),...
|
||||
odom.Time(odomIndex(1):end),odom.Data(odomIndex(1):end,2),...
|
||||
cmd_vel_safe.Time(vindices(1):end),cmd_vel_safe.Data(vindices(1):end,1),...
|
||||
cmd_vel_safe.Time(vindices(1):end),cmd_vel_safe.Data(vindices(1):end,2));
|
||||
legend({'X position','Y position','cmd velocity','cmd steering'})
|
||||
|
||||
%% visualize the (x,y) position only
|
||||
figure
|
||||
plot(odom.Data(:,1),odom.Data(:,2))
|
||||
axis equal
|
||||
legend('(x,y) position');
|
||||
xlabel('X position')
|
||||
ylabel('Y position')
|
|
@ -0,0 +1,73 @@
|
|||
%Implementation of follower algorithm
|
||||
|
||||
%Developed by Rahul Kumar Bhadani <rahulbhadani@email.arizona.edu>
|
||||
|
||||
%ROS_IP = IP Address of ROS Master
|
||||
%lead = name of the model of leader AV Car
|
||||
%follower = name of the model of follower car
|
||||
|
||||
function profileByMatrix(ROS_IP, roboname, vel_input, time_input, tire_angle)
|
||||
|
||||
|
||||
|
||||
%If number of argument is not two, flag message and exit.
|
||||
if nargin < 4
|
||||
sprintf('Uage: velocityProfiler(192.168.0.32, catvehicle, velmatfile, timematfile)');
|
||||
return;
|
||||
end
|
||||
|
||||
if nargin < 5
|
||||
tire_angle = 0.0;
|
||||
end
|
||||
rosshutdown;
|
||||
close all;
|
||||
modelname = strcat('/',roboname);
|
||||
%Connect to ROS master
|
||||
master_uri= strcat('http://',ROS_IP);
|
||||
master_uri = strcat(master_uri,':11311');
|
||||
rosinit(master_uri);
|
||||
|
||||
%get handle for /catvehicle/cmd_vel topic for publishing the data
|
||||
velpub = rospublisher(strcat(modelname,'/cmd_vel'),rostype.geometry_msgs_Twist);
|
||||
|
||||
%get handle for /catvehicle/vel topic for subscribing to the data
|
||||
speedsub = rossubscriber(strcat(modelname,'/vel'));
|
||||
|
||||
vmat = load(vel_input);
|
||||
tmat = load(time_input);
|
||||
t = tmat.t;
|
||||
%Velocity profile
|
||||
input = vmat.Vel;
|
||||
%Velocity profile will be sine
|
||||
%input = abs(2*sin(t));
|
||||
|
||||
%Variable to store output velocity
|
||||
output = zeros(length(t),1);
|
||||
|
||||
%handle for rosmessage object for velpub topic
|
||||
velMsgs = rosmessage(velpub);
|
||||
for i=1:length(t)
|
||||
velMsgs.Linear.X = input(i);
|
||||
velMsgs.Angular.Z = tire_angle;
|
||||
%Publish on the topic /catvehicle/cmd_vel
|
||||
send(velpub, velMsgs);
|
||||
%Read from the topic /catvehicle/speed
|
||||
speedata = receive(speedsub,10);
|
||||
output(i) = speedata.Linear.X;
|
||||
pause(0.1);
|
||||
if i == 3000
|
||||
break;
|
||||
end
|
||||
end
|
||||
|
||||
%Plot the input and output velocity profile
|
||||
[n, p] = size(output);
|
||||
T = 1:n;
|
||||
plot(T, input');
|
||||
hold on;
|
||||
plot(T, output);
|
||||
title('Original Data');
|
||||
legend('Input function', 'Output response');
|
||||
grid on;
|
||||
|
||||
save input.mat input output
|
|
@ -0,0 +1,70 @@
|
|||
function retcode = ros2mat(bagfile)
|
||||
%ROS2MAT Open a rosbag file and convert it into a mat file.
|
||||
% (c) Author: Rahul Kumar Bhadani
|
||||
% Copyright (c) 2018 Arizona Board of Regents
|
||||
% See copyright file
|
||||
%
|
||||
% ROS2MAT('FILEPATH') parses all the data types and extracts
|
||||
% the time series data. These timeseries data are saved as
|
||||
% .mat file in the same directory where bag file is located.
|
||||
% This function returns an integer specifying success/error code.
|
||||
% Return code 1: Success
|
||||
% Return code -1: Input file doesn't exist
|
||||
% Return code -2: Input file is not a bag file.
|
||||
% Return code -3: Input file with a .bag extension is not a proper bag
|
||||
% file.
|
||||
% Return code -4: One or more message type is unknown by MATLAB.
|
||||
%
|
||||
% Example:
|
||||
% Open a robsbag file and convert it to a mat file
|
||||
% Ret = ros2mat('2017-01-01-11-11-11.bag')
|
||||
|
||||
|
||||
if ((exist(bagfile,'file') == 2))
|
||||
|
||||
%Check of the given file has bag file extension
|
||||
if( strcmp(bagfile(end-3:end),'.bag') == 0)
|
||||
retcode = -2;
|
||||
fprintf("\nFile '%s' is not a rosbag file.\n\n",bagfile);
|
||||
else
|
||||
try
|
||||
bag = rosbag(bagfile);
|
||||
%Get the list of topics
|
||||
topics= bag.AvailableTopics;
|
||||
%Retrieve the number of topics.
|
||||
num_topics = height(topics);
|
||||
bagStruct = struct;
|
||||
for i =1:num_topics
|
||||
fprintf("\n%d. Reading %s\n",i, char(topics.Row(i)));
|
||||
%Select the i-th topic from bag container.
|
||||
try
|
||||
%Save the topic name corresponding to the timeseries data
|
||||
%so as to keep the context/meaning.
|
||||
bagStruct(i).topicname = topics.Row(i);
|
||||
bagStruct(i).topictype = topics.MessageType(i);
|
||||
bagSelect = select(bag, 'Topic', char(topics.Row(i)));
|
||||
fprintf("\nNumber of messages in %s is %f\n\n",char(topics.Row(i)), bagSelect.NumMessages);
|
||||
%Extract the timeseries data and save in a structure member
|
||||
bagStruct(i).data = timeseries(bagSelect);
|
||||
catch ME
|
||||
if(strcmp(ME.identifier, 'robotics:ros:message:NoMatlabClass'))
|
||||
fprintf("MATLAB doesn't know this data type and can't parse it.\n");
|
||||
retcode = -4;
|
||||
end
|
||||
end
|
||||
end
|
||||
save(strcat(bagfile(1:end-4),'.mat'),'bagStruct');
|
||||
retcode = 1;
|
||||
catch
|
||||
retcode = -3;
|
||||
fprintf("\nUnable to read %s, input file may not be a proper bagfile.\n\n",...
|
||||
bagfile);
|
||||
|
||||
end
|
||||
end
|
||||
else
|
||||
retcode = -1;
|
||||
fprintf("\nFile '%s' doesn't exist\n\n",bagfile);
|
||||
end
|
||||
end
|
||||
|
|
@ -0,0 +1,117 @@
|
|||
% Author: Rahul Kumar Bhadani
|
||||
% rahulbhadani@email.arizona.edu
|
||||
% This file will read the velocity data from the ascii file and clip the
|
||||
% portion of data that are recorded before the input time so that we can
|
||||
% shift graph at t = t0 when experiment starts
|
||||
|
||||
|
||||
|
||||
% Load the data from file into matlab variable assuming that current
|
||||
% directory has the data file named vData.mat. If you are working with file
|
||||
% that has different name, please change the script
|
||||
|
||||
%Another assumption has been made that output start from zero and slowly
|
||||
%gain the value.
|
||||
data = load('vData.mat','-ascii');
|
||||
%Step Input: Change the value as required
|
||||
A =1;
|
||||
|
||||
[n, p] = size(data);
|
||||
t = 1:n;
|
||||
Input=repmat(A,1,n);
|
||||
figure(1);
|
||||
plot(t, Input');
|
||||
hold on;
|
||||
plot(t, data);
|
||||
title('Original Data');
|
||||
legend('Input Step function', 'Output response');
|
||||
grid on;
|
||||
|
||||
%determine the size of data file
|
||||
first_zero = 0;
|
||||
last_zero = 0;
|
||||
final_time = 0;
|
||||
|
||||
%Clip the region before the start of the experiment
|
||||
for t=1:length(data)
|
||||
if( data(t) > 0.0 && data (t) < 0.0001)
|
||||
first_zero = t;
|
||||
end
|
||||
end
|
||||
|
||||
data = data(first_zero:length(data));
|
||||
|
||||
for t=1:length(data)
|
||||
if( data(t+5) - data (t) > 0.0005)
|
||||
last_zero = t;
|
||||
break;
|
||||
end
|
||||
end
|
||||
|
||||
data = data(last_zero:length(data));
|
||||
|
||||
for t=1:length(data)
|
||||
if( abs(data(t+10) - data (t)) < 0.000001)
|
||||
final_time = t+10;
|
||||
break;
|
||||
end
|
||||
end
|
||||
|
||||
data = data(1:final_time);
|
||||
|
||||
%At this point we have the data that are from experiment.
|
||||
[n, p] = size(data);
|
||||
t = 1:n;
|
||||
Input=repmat(A,1,n);
|
||||
figure(2);
|
||||
plot(t, Input');
|
||||
hold on;
|
||||
plot(t, data);
|
||||
title('Clean Data');
|
||||
legend('Input Step function', 'Output response');
|
||||
grid on;
|
||||
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
%Approximation of data as second order system to determine the transfer
|
||||
%function
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
%Get the peak time.
|
||||
[maxVal, maxTime] = max(data);
|
||||
peakTime = maxTime;
|
||||
hold on;
|
||||
plot(peakTime, maxVal,'*');
|
||||
text(peakTime, maxVal,'Y_{peak}');
|
||||
finalVal = 0;
|
||||
finalTime = 0;
|
||||
%Find the final value. If the amplitude has not changed more than threshold
|
||||
%within last 50
|
||||
%unit of time, treat the latter amplitude as final value.
|
||||
for t=peakTime:length(data)
|
||||
if( abs(data(t) - data (t-25)) < A*0.0001)
|
||||
finalVal = data(t);
|
||||
finalTime = t;
|
||||
break;
|
||||
end
|
||||
end
|
||||
|
||||
plot(finalTime, finalVal,'*');
|
||||
text(finalTime, finalVal,'Y_{final}');
|
||||
|
||||
%Overshoot
|
||||
Overshoot = (maxVal - finalVal)/finalVal;
|
||||
%Damping Coefficient
|
||||
Zeta = sqrt( (log(Overshoot)^2) / ( (log(Overshoot)^2) +(pi^2) ) );
|
||||
|
||||
Wd = pi/peakTime;
|
||||
Wn = Wd/( sqrt ( 1 - (Zeta^2) ) );
|
||||
|
||||
denom = [1 2*Zeta*Wn Wn*Wn];
|
||||
sys = tf(A, denom);
|
||||
t = [0:0.01:1000];
|
||||
figure(3);
|
||||
title('Approximation of response as second order system');
|
||||
step(sys,t);
|
||||
grid on;
|
|
@ -0,0 +1,39 @@
|
|||
% Function to capture catvehicle velocity and plotting live graph
|
||||
function velCapture(ROS_IP, roboname)
|
||||
%If number of argument is not two, flag message and exit.
|
||||
if nargin < 2
|
||||
disp('Uage: velocityProfiler(192.168.0.32, catvehicle)');
|
||||
return;
|
||||
end
|
||||
close all;
|
||||
%rosshutdown;
|
||||
modelname = strcat('/',roboname);
|
||||
%Connect to ROS master
|
||||
master_uri= strcat('http://',ROS_IP);
|
||||
master_uri = strcat(master_uri,':11311');
|
||||
%rosinit(master_uri);
|
||||
|
||||
%get handle for /catvehicle/vel topic for subscribing to the data
|
||||
speedsub = rossubscriber(strcat(modelname,'/vel'));
|
||||
dt = datestr(now,'mmmm-dd-yyyy-HH-MM-SS');
|
||||
sprintf('Velocity capture starts at %s',dt)
|
||||
|
||||
t = 0:0.05:50;
|
||||
output = zeros(length(t),1);
|
||||
figure;
|
||||
grid on;
|
||||
title('Velocity [m/s]');
|
||||
for i = 1:length(t)
|
||||
speedata = receive(speedsub,10);
|
||||
output(i) = speedata.Linear.X;
|
||||
|
||||
plot([max(i-1,1),i], output([max(i-1,1),i]),'b-');
|
||||
hold on;
|
||||
drawnow;
|
||||
end
|
||||
dt = datestr(now,'mmmm-dd-yyyy-HH-MM-SS');
|
||||
file = strcat(dt,'.mat');
|
||||
save(file, 'output');
|
||||
grid on;
|
||||
title('Velocity [m/s]');
|
||||
end
|
|
@ -0,0 +1,69 @@
|
|||
%Matlab scripto to publish velocity on /catvehicle/cmd_vel topic and
|
||||
%subscribe to catvehicle/speed topic
|
||||
|
||||
%Developed by Rahul Kumar Bhadani <rahulbhadani@email.arizona.edu>
|
||||
|
||||
%ROS_IP = IP Address of ROS Master
|
||||
%roboname = name of the model
|
||||
function velocityProfiler(ROS_IP, roboname, tire_angle)
|
||||
|
||||
|
||||
%If number of argument is not two, flag message and exit.
|
||||
if nargin < 2
|
||||
sprintf('Uage: velocityProfiler(192.168.0.32, catvehicle)');
|
||||
return;
|
||||
end
|
||||
|
||||
if nargin < 3
|
||||
tire_angle = 0.0;
|
||||
end
|
||||
rosshutdown;
|
||||
close all;
|
||||
modelname = strcat('/',roboname);
|
||||
%Connect to ROS master
|
||||
master_uri= strcat('http://',ROS_IP);
|
||||
master_uri = strcat(master_uri,':11311');
|
||||
rosinit(master_uri);
|
||||
|
||||
%get handle for /catvehicle/cmd_vel topic for publishing the data
|
||||
velpub = rospublisher(strcat(modelname,'/cmd_vel'),rostype.geometry_msgs_Twist);
|
||||
|
||||
%get handle for /catvehicle/vel topic for subscribing to the data
|
||||
speedsub = rossubscriber(strcat(modelname,'/vel'));
|
||||
|
||||
%Discretize timestamp
|
||||
t = 0:0.01:150;
|
||||
v1 = 3;
|
||||
v2 = 6;
|
||||
v3 = 0;
|
||||
|
||||
%Velocity profile
|
||||
input = v1.*(t<50) + v2.*(t>=50).*(t<100) + v3.*(t>= 100);
|
||||
|
||||
%Velocity profile will be sine
|
||||
%input = abs(2*sin(t));
|
||||
|
||||
%Variable to store output velocity
|
||||
output = zeros(length(t),1);
|
||||
|
||||
%handle for rosmessage object for velpub topic
|
||||
velMsgs = rosmessage(velpub);
|
||||
for i=1:length(t)
|
||||
velMsgs.Linear.X = input(i);
|
||||
velMsgs.Angular.Z = tire_angle;
|
||||
%Publish on the topic /catvehicle/cmd_vel
|
||||
send(velpub, velMsgs);
|
||||
%Read from the topic /catvehicle/speed
|
||||
speedata = receive(speedsub,10);
|
||||
output(i) = speedata.Linear.X;
|
||||
end
|
||||
|
||||
%Plot the input and output velocity profile
|
||||
[n, p] = size(output);
|
||||
T = 1:n;
|
||||
plot(T, input');
|
||||
hold on;
|
||||
plot(T, output);
|
||||
title('Original Data');
|
||||
legend('Input function', 'Output response');
|
||||
grid on;
|
|
@ -0,0 +1,85 @@
|
|||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<name>catvehicle</name>
|
||||
<version>2.1.0</version>
|
||||
<description>The catvehicle package (formerly known as azcar_sim)</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="sprinkjm@email.arizona.edu">Jonathan Sprinkle</maintainer>
|
||||
<maintainer email="rahulbhadani@email.arizona.edu">Rahul Kumar Bhadani</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>BSD</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/catvehicle</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, mutiple are allowed, one per tag -->
|
||||
<author email="sprinkjm@email.arizona.edu">Jonathan Sprinkle</author>
|
||||
<author email="rahulkumarbhadani@email.arizona.edu">Rahul Bhadani</author>
|
||||
<author>Sam Taylor</author>
|
||||
<author email="kennondmckeever@email.arizona.edu">Kennon McKeever</author>
|
||||
<author>Alex Warren</author>
|
||||
<author email="smunjal@email.arizona.edu">Swati Munjal</author>
|
||||
<author email="askang@email.arizona.edu">Ashley Kang</author>
|
||||
<author email="mosfet@email.arizona.edu">Matt Bunting</author>
|
||||
<author>Sean Whitsitt</author>
|
||||
|
||||
<!-- The *_depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<!-- <build_depend>message_generation</build_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use run_depend for packages you need at runtime: -->
|
||||
<!-- <run_depend>message_runtime</run_depend> -->
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>controller_manager</build_depend>
|
||||
<build_depend>gazebo_ros_control</build_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>position_controllers</build_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>sensor_msgs</build_depend>
|
||||
<build_depend>sicktoolbox</build_depend>
|
||||
<build_depend>sicktoolbox_wrapper</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>tf</build_depend>
|
||||
<build_depend>transmission_interface</build_depend>
|
||||
<build_depend>velocity_controllers</build_depend>
|
||||
<build_depend>velodyne_pointcloud</build_depend>
|
||||
<run_depend>controller_manager</run_depend>
|
||||
<run_depend>gazebo_ros_control</run_depend>
|
||||
<run_depend>geometry_msgs</run_depend>
|
||||
<run_depend>position_controllers</run_depend>
|
||||
<run_depend>roscpp</run_depend>
|
||||
<run_depend>rospy</run_depend>
|
||||
<run_depend>sensor_msgs</run_depend>
|
||||
<run_depend>sicktoolbox</run_depend>
|
||||
<run_depend>sicktoolbox_wrapper</run_depend>
|
||||
<run_depend>std_msgs</run_depend>
|
||||
<run_depend>obstaclestopper</run_depend>
|
||||
<run_depend>tf</run_depend>
|
||||
<run_depend>transmission_interface</run_depend>
|
||||
<run_depend>velocity_controllers</run_depend>
|
||||
<run_depend>velodyne_pointcloud</run_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
|
@ -0,0 +1,12 @@
|
|||
## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
|
||||
|
||||
from distutils.core import setup
|
||||
from catkin_pkg.python_setup import generate_distutils_setup
|
||||
|
||||
# fetch values from package.xml
|
||||
setup_args = generate_distutils_setup(
|
||||
packages=['catvehicle'],
|
||||
package_dir={'': 'src'},
|
||||
)
|
||||
|
||||
setup(**setup_args)
|
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
@ -0,0 +1,12 @@
|
|||
%% my test file
|
||||
% To visualize the angles, run this code:
|
||||
radius=3.6; start=[0 0];
|
||||
angles=linspace(-pi,pi,31);
|
||||
r = radius*exp(angles*1j);
|
||||
xs = real(r)-radius+start(1);
|
||||
ys = imag(r)+start(2);
|
||||
% since the direction of travel is in the 'x' direction, the
|
||||
% heading (phi) matchines the angle of the circle
|
||||
phis=(pi/2)+angles;
|
||||
xends=cos(phis);yends=sin(phis);
|
||||
figure;plot(xs,ys);hold on; quiver(xs,ys,xends,yends);axis equal;hold off
|
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
@ -0,0 +1,16 @@
|
|||
% Author: Jonathan Sprinkle
|
||||
% plots the distance outputs from a data file
|
||||
|
||||
function plotData( timeseries )
|
||||
|
||||
% this timeseries is what we have
|
||||
figure
|
||||
hold on
|
||||
plot(timeseries.dist);
|
||||
plot(timeseries.velConverted);
|
||||
plot(timeseries.vdot);
|
||||
plot(timeseries.vout);
|
||||
plot(timeseries.uTireAngle);
|
||||
legend({'dist','velConverted','vdot','vout','uTireAngle'});
|
||||
|
||||
end
|
|
@ -0,0 +1,13 @@
|
|||
% Author: Jonathan Sprinkle
|
||||
% plots the distance outputs from a data file
|
||||
|
||||
function plotDistances
|
||||
load distances.mat
|
||||
% this timeseries is what we have
|
||||
figure
|
||||
hold on
|
||||
plot(DistanceEstimator.Data__signal_1_);
|
||||
plot(DistanceEstimator.Data__signal_2_);
|
||||
legend({'Distance','Angle (rad)'});
|
||||
|
||||
end
|
|
@ -0,0 +1,13 @@
|
|||
% Author: Jonathan Sprinkle
|
||||
% plots the distance outputs from a data file
|
||||
|
||||
function plotData( timeseries )
|
||||
|
||||
% this timeseries is what we have
|
||||
figure
|
||||
hold on
|
||||
plot(timeseries.Data);
|
||||
plot(timeseries.uVelOut);
|
||||
legend({'Distance','VelOut'});
|
||||
|
||||
end
|
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue