├── .gitignore ├── .travis.rosinstall ├── .travis.yml ├── LICENSE ├── README.md ├── m1n6s200_segbot_moveit_config ├── .setup_assistant ├── CMakeLists.txt ├── config │ ├── controllers.yaml │ ├── controllers_ros_control.yaml │ ├── fake_controllers.yaml │ ├── joint_limits.yaml │ ├── joint_names.yaml │ ├── kinematics.yaml │ ├── ompl_planning.yaml │ └── segbot.srdf ├── launch │ ├── default_warehouse_db.launch │ ├── demo.launch │ ├── fake_moveit_controller_manager.launch.xml │ ├── joystick_control.launch │ ├── m1n6s200_demo.launch │ ├── m1n6s200_gazebo_demo.launch │ ├── m1n6s200_moveit_controller_manager.launch.xml │ ├── m1n6s200_moveit_sensor_manager.launch.xml │ ├── m1n6s200_ros_control_moveit_controller_manager.launch.xml │ ├── m1n6s200_segbot_moveit_bringup.launch │ ├── m1n6s200_segbot_moveit_simulation.launch │ ├── m1n6s200_virtual_robot_demo.launch │ ├── move_group.launch │ ├── move_group_m1n6s200.launch │ ├── moveit.rviz │ ├── moveit_rviz.launch │ ├── ompl_planning_pipeline.launch.xml │ ├── planning_context.launch │ ├── planning_pipeline.launch.xml │ ├── run_benchmark_ompl.launch │ ├── sensor_manager.launch.xml │ ├── setup_assistant.launch │ ├── trajectory_execution.launch.xml │ ├── warehouse.launch │ └── warehouse_settings.launch.xml └── package.xml ├── only_segbot_arm ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── only_segbot_base ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── segbot ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── segbot_arm_demos ├── draw_demo │ ├── CMakeLists.txt │ ├── config │ │ ├── common_angles.txt │ │ └── establish_contact_efforts.txt │ ├── launch │ │ ├── approach_board.launch │ │ ├── draw_demo.launch │ │ └── segmentation.launch │ ├── opencv │ │ ├── CMakeLists.txt │ │ ├── IMG_5465-1.jpg │ │ ├── Makefile │ │ ├── WEDDING.cdcode │ │ ├── asd.cdcode │ │ ├── barn.cdcode │ │ ├── barn.png │ │ ├── barn2.cdcode │ │ ├── barn2.png │ │ ├── building.jpg │ │ ├── bwi.png │ │ ├── bwi2.cdcode │ │ ├── circle.cdcode │ │ ├── circle.png │ │ ├── cmake_install.cmake │ │ ├── cube.cdcode │ │ ├── cube.png │ │ ├── cube2.cdcode │ │ ├── drawing_files │ │ │ ├── barn.cdcode │ │ │ ├── barn2.cdcode │ │ │ ├── bwi.cdcode │ │ │ ├── bwi2.cdcode │ │ │ ├── bwi3.cdcode │ │ │ ├── circle.cdcode │ │ │ ├── cube.cdcode │ │ │ ├── cube2.cdcode │ │ │ ├── house.cdcode │ │ │ ├── longhorn.cdcode │ │ │ ├── longhorn2.cdcode │ │ │ ├── longhorn3.cdcode │ │ │ ├── max.cdcode │ │ │ ├── maxwell.cdcode │ │ │ ├── mona.cdcode │ │ │ ├── s.cdcode │ │ │ └── wow.cdcode │ │ ├── findContours │ │ ├── findContours.cpp │ │ ├── house2.png │ │ ├── house3.cdcode │ │ ├── house3.png │ │ ├── htransformp.cpp │ │ ├── longhorn.cdcode │ │ ├── longhorn2.cdcode │ │ ├── longhorn2.jpg │ │ ├── longhornRE.cdcode │ │ ├── maxwell.cdcode │ │ ├── maxwell.png │ │ ├── mona.cdcode │ │ ├── mona.jpg │ │ ├── test2.cdcode │ │ ├── transforms │ │ ├── wedding_resized.jpg │ │ ├── wow.cdcode │ │ └── wow.png │ ├── package.xml │ └── src │ │ ├── approach_board.cpp │ │ ├── distPub.cpp │ │ ├── draw.cpp │ │ ├── moveFingers.cpp │ │ ├── movement_demo.cpp │ │ ├── recordPoints.cpp │ │ ├── recordTrajectory.cpp │ │ ├── shapes.cpp │ │ └── visionDistance.cpp └── mimic_motion │ ├── CMakeLists.txt │ ├── package.xml │ └── src │ ├── candy_demo.cpp │ ├── push_button_demo.cpp │ ├── touch_detect_tabletop_demo.cpp │ └── touch_tabletop_object_demo.cpp ├── segbot_arm_joystick ├── CMakeLists.txt ├── README.md ├── launch │ └── segbot_arm_joystick.launch ├── package.xml └── src │ └── segbot_arm_joystick.cpp ├── segbot_arm_manipulation ├── CMakeLists.txt ├── action │ ├── Handover.action │ ├── LiftVerify.action │ ├── ObjReplacement.action │ ├── Press.action │ ├── Push.action │ ├── Shake.action │ ├── TabletopApproach.action │ └── TabletopGrasp.action ├── data │ ├── mico_jointspace_position_db.txt │ └── mico_toolspace_position_db.txt ├── include │ └── segbot_arm_manipulation │ │ ├── Mico.h │ │ ├── arm_positions_db.h │ │ └── arm_utils.h ├── launch │ ├── segbot_arm_explore.launch │ └── segbot_arm_manipulation.launch ├── package.xml ├── scripts │ └── arm_actions.py ├── setup.py ├── src │ ├── Mico.cpp │ ├── arm_actions.py │ ├── arm_lift_verify_as.cpp │ ├── arm_press_as.cpp │ ├── arm_push_as.cpp │ ├── arm_shake_as.cpp │ ├── arm_utils.cpp │ ├── demos │ │ ├── demo_explore_object.cpp │ │ ├── demo_gestures.cpp │ │ ├── demo_grasp_action_client.cpp │ │ ├── demo_grasp_and_lay_down.cpp │ │ ├── demo_grasp_verify.cpp │ │ ├── demo_hand_lead.cpp │ │ ├── demo_obstacle_avoidance.cpp │ │ ├── demo_press_button.cpp │ │ ├── object_to_office_task.cpp │ │ ├── test_grasp_replacement.cpp │ │ ├── test_handover_replacement.cpp │ │ ├── test_ispy.cpp │ │ └── test_table_approach_actions.cpp │ ├── ispy_arm_server.cpp │ ├── nav_safety_service.cpp │ ├── position_record.cpp │ ├── segbot_arm_manipulation │ │ └── __init__.py │ ├── segbot_handover_as.cpp │ ├── segbot_obj_replacement_as.cpp │ ├── segbot_table_approach_as.cpp │ ├── segbot_tabletop_grasp_as.cpp │ └── trajectory_record.cpp └── srv │ ├── NavSafety.srv │ ├── iSpyDetectTouch.srv │ ├── iSpyFaceTable.srv │ ├── iSpyReorderClouds.srv │ └── iSpyTouch.srv ├── segbot_arm_tasks ├── CMakeLists.txt ├── package.xml └── src │ ├── object_handover_delivery_task.cpp │ ├── object_table_delivery_task.cpp │ └── object_to_office_task.cpp ├── segbot_arm_tutorials ├── CMakeLists.txt ├── package.xml └── src │ ├── ex1_subscribing_to_topics.cpp │ ├── ex2_gripper_control.cpp │ ├── ex3_home_arm.cpp │ ├── ex4_cartesian_vel_control.cpp │ ├── ex5_angular_vel_control.cpp │ ├── ex6_detect_force.cpp │ ├── ex7_cartesian_pose_control.cpp │ ├── ex8_joint_angles_control.cpp │ └── ex9_waypoints.cpp ├── segbot_description ├── CHANGELOG.rst ├── CMakeLists.txt ├── gazebo │ └── segbot.gazebo.xacro ├── meshes │ ├── hokuyo.dae │ ├── hokuyo.png │ ├── kinect.dae │ ├── rmp50-base-link.dae │ ├── rmp50-castor-link.dae │ ├── rmp50-castor-wheel.dae │ ├── rmp50-left-wheel.dae │ ├── rmp50-right-wheel.dae │ └── xtion │ │ ├── xtion_mount_base_link.STL │ │ └── xtion_pivot_link.STL ├── package.xml ├── robots │ ├── segbot_v1.hokuyo_only.urdf.xacro │ ├── segbot_v1.no_sensors.urdf.xacro │ ├── segbot_v1.urdf.xacro │ ├── segbot_v2.arm.urdf.xacro │ ├── segbot_v2.urdf.xacro │ ├── segbot_v3.urdf.xacro │ └── segbot_v4.urdf.xacro ├── test │ ├── hokuyo_mount.test.xacro │ ├── kinect_mount.test.xacro │ ├── segbot_chassis.test.xacro │ ├── urdf.rviz │ ├── velodyne_mount.test.xacro │ └── xacrodisplay.launch └── urdf │ ├── components │ ├── alienware_alpha.urdf.xacro │ ├── aluminium_sheet.urdf.xacro │ ├── battery_box.urdf.xacro │ ├── beam_8020.urdf.xacro │ ├── box_laptop.urdf.xacro │ ├── laptop.urdf.xacro │ └── standoff.urdf.xacro │ ├── mounts │ ├── hokuyo_mount.urdf.xacro │ ├── kinect_mount.urdf.xacro │ ├── sensor_mount.urdf.xacro │ ├── sensor_plate_mount.urdf.xacro │ └── velodyne_mount.urdf.xacro │ ├── segbot_base.arm.urdf.xacro │ ├── segbot_base.urdf.xacro │ ├── segbot_wheel.urdf.xacro │ ├── segway_110_base.urdf.xacro │ ├── sensors │ ├── VLP-16.urdf.xacro │ ├── hokuyo_laser.gazebo.xacro │ ├── hokuyo_laser.urdf.xacro │ ├── kinect_camera.gazebo.xacro │ ├── kinect_camera.urdf.xacro │ ├── xtion_camera.gazebo.xacro │ └── xtion_camera.urdf.xacro │ ├── v1 │ ├── common.urdf.xacro │ └── segbot_chassis.urdf.xacro │ ├── v2 │ ├── common.urdf.xacro │ ├── segbot_chassis.urdf.arm.xacro │ └── segbot_chassis.urdf.xacro │ └── v3 │ ├── common.urdf.xacro │ └── segbot_chassis.urdf.xacro ├── segbot_firmware ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── package.xml └── src │ └── libraries │ ├── CalLib │ ├── CalLib.cpp │ └── CalLib.h │ ├── I2CDev │ ├── I2Cdev.cpp │ └── I2Cdev.h │ ├── MPU9150Lib │ ├── MPU9150Lib.cpp │ ├── MPU9150Lib.h │ ├── MPUQuaternion.cpp │ ├── MPUQuaternion.h │ ├── MPUVector3.cpp │ └── MPUVector3.h │ ├── MotionDriver │ ├── dmpKey.h │ ├── dmpmap.h │ ├── inv_mpu.cpp │ ├── inv_mpu.h │ ├── inv_mpu_dmp_motion_driver.cpp │ └── inv_mpu_dmp_motion_driver.h │ ├── NewPing │ ├── NewPing.cpp │ ├── NewPing.h │ └── keywords.txt │ ├── imu │ ├── imu.cpp │ └── imu.h │ ├── mpu9150_only │ └── mpu9150_only.ino │ ├── multidevice │ ├── arduino_device.h │ └── multidevice.ino │ ├── segbot1 │ ├── README.md │ └── segbot1.ino │ ├── segbot_arduino │ ├── README.md │ └── segbot_arduino.ino │ ├── sonar │ ├── sonar.cpp │ └── sonar.h │ ├── voltmeter │ ├── voltmeter.cpp │ └── voltmeter.h │ └── volts_only │ ├── arduino_device.h │ └── volts_only.ino ├── segbot_gazebo ├── CHANGELOG.rst ├── CMakeLists.txt ├── Media │ └── models │ │ ├── segbot_test_world.dae │ │ └── segbot_test_world │ │ ├── Blinds_Vertical_Pattern_.jpg │ │ ├── Brick_Antique_.jpg │ │ └── Wood_Bamboo_Medium_.jpg ├── config │ └── multiple_segbots.rviz ├── launch │ ├── multiple_segbots.launch │ ├── segbot_mobile_base.launch │ ├── segbot_navigation.launch │ ├── segbot_test_world.launch │ └── segbot_test_world_map_server.launch ├── maps │ ├── segbot_test_world.pgm │ └── segbot_test_world.yaml ├── package.xml └── worlds │ └── segbot_test_world.world ├── segbot_led ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── arduino │ ├── README.md │ ├── led_serial_driver │ │ └── led_serial_driver.ino │ └── pololu-led-strip-arduino │ │ ├── LICENSE.txt │ │ ├── PololuLedStrip │ │ ├── PololuLedStrip.cpp │ │ ├── PololuLedStrip.h │ │ └── examples │ │ │ ├── LedStripColorTester │ │ │ └── LedStripColorTester.ino │ │ │ ├── LedStripGradient │ │ │ └── LedStripGradient.ino │ │ │ ├── LedStripRainbow │ │ │ └── LedStripRainbow.ino │ │ │ └── LedStripXmas │ │ │ └── LedStripXmas.ino │ │ └── README.textile ├── images │ ├── back.JPG │ ├── front.JPG │ ├── left.JPG │ └── right.JPG ├── launch │ └── segbot_led_v3.launch ├── ledcomm │ ├── ledcom.h │ └── rgbhsv.h ├── package.xml ├── scripts │ └── udev_install.sh ├── src │ └── led_control_server.cpp ├── test │ └── test.cpp └── udev │ ├── 99-metromini.rules │ └── README.md ├── segbot_navigation ├── CHANGELOG.rst ├── CMakeLists.txt ├── config │ ├── segbotv2 │ │ ├── costmap_common_params.yaml │ │ ├── eband_planner_params.yaml │ │ ├── global_costmap_params.yaml │ │ ├── local_costmap_params.yaml │ │ └── move_base_params.yaml │ ├── segbotv3 │ │ ├── costmap_common_params.yaml │ │ ├── eband_planner_params.yaml │ │ ├── global_costmap_params.yaml │ │ ├── local_costmap_params.yaml │ │ └── move_base_params.yaml │ └── segbotv4 │ │ ├── costmap_common_params.yaml │ │ ├── eband_planner_params.yaml │ │ ├── global_costmap_params.yaml │ │ ├── local_costmap_params.yaml │ │ └── move_base_params.yaml ├── launch │ ├── README.md │ ├── amcl.launch │ ├── gmapping.launch │ ├── gmapping_v3.launch │ ├── move_base_eband.launch │ ├── navigation.launch │ ├── navigation_v2.launch │ ├── robot_with_gmapping_v2.launch │ ├── robot_with_gmapping_v3.launch │ └── rviz.launch ├── package.xml ├── rviz │ ├── nav_eband.rviz │ └── nav_eband_marvin.rviz ├── scripts │ ├── move_base_interruptable_simple │ └── rviz_runner └── src │ └── move_base_interruptable_server.cpp ├── segbot_sensors ├── .gitignore ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.rst ├── cfg │ └── SegbotVelodyneOutlierRemoval.cfg ├── config │ ├── diagnostics_analyzer.yaml │ ├── hokuyo_filters.yaml │ ├── hokuyo_filters_v2.yaml │ ├── kinect_filters.yaml │ ├── v3_diagnostics_analyzer.yaml │ └── velodyne_filters.yaml ├── include │ └── segbot_sensors │ │ ├── angle_range_filter.h │ │ ├── footprint_filter.h │ │ ├── nan_to_inf_filter.h │ │ └── segbot_velodyne_outlier_removal.h ├── launch │ ├── arduino │ │ └── arduino.launch │ ├── diagnostic │ │ └── diagnostics.launch │ ├── hokuyo │ │ ├── eth_hokuyo.launch │ │ ├── hokuyo-filters.launch │ │ ├── hokuyo-frames.launch │ │ ├── hokuyo.launch │ │ └── urg_node.launch │ ├── kinect │ │ ├── kinect-filters.launch │ │ ├── kinect-laserscan.launch │ │ └── kinect.launch │ ├── ptgrey │ │ └── usbcam.launch │ ├── rviz.launch │ ├── velodyne │ │ ├── velodyne-filters.launch │ │ ├── velodyne-laserscan.launch │ │ └── velodyne.launch │ └── xtion.launch ├── nodelets.xml ├── nodes │ └── arduino_driver ├── package.xml ├── rviz │ └── sensors.rviz ├── segbot_sensors_plugins.xml ├── sendmail_configure.sh ├── setup.py ├── src │ ├── segbot_sensors │ │ ├── __init__.py │ │ ├── arduino_driver_node.py │ │ ├── battery_diagnostics.cpp │ │ ├── battery_profiler.py │ │ ├── diagnostics.py │ │ ├── fake_volt_pub.cpp │ │ ├── imu.py │ │ ├── serial.py │ │ ├── sonar.py │ │ └── voltmeter.py │ ├── segbot_sensors_filters.cpp │ ├── segbot_velodyne_outlier_removal.cpp │ └── sonar │ │ ├── CMakeLists.txt │ │ ├── range_nodelet.cpp │ │ ├── range_to_cloud.cpp │ │ └── range_to_cloud.h └── test │ ├── 7_arduino_sensors.txt │ ├── arduino_ir_test.txt │ ├── imu_msg_test.txt │ ├── named-hokuyo-ns.launch │ ├── named-kinect-ns.launch │ ├── poll_cycle_arduino.txt │ ├── sonar_msg_test.txt │ ├── test_arduino.launch │ └── voltage_msg_test.txt └── segbot_simulation_apps ├── CHANGELOG.rst ├── CMakeLists.txt ├── include └── segbot_simulation_apps │ ├── common.h │ └── door_handler.h ├── launch └── door_handler.launch ├── package.xml ├── src ├── libsegbot_simulation_apps │ ├── common.cpp │ └── door_handler.cpp └── nodes │ ├── door_handler.cpp │ └── robot_teleporter.cpp └── urdf ├── door.urdf └── obstacle.urdf /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | bin 3 | build 4 | lib 5 | *.swp 6 | *.swo 7 | *.pyc 8 | .idea 9 | cmake-build-debug 10 | -------------------------------------------------------------------------------- /.travis.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: agile_grasp 3 | uri: https://github.com/utexas-bwi/agile_grasp.git 4 | version: melodic-devel 5 | - git: 6 | local-name: bwi_common 7 | uri: https://github.com/utexas-bwi/bwi_common.git 8 | version: devel 9 | - git: 10 | local-name: kinova-ros 11 | uri: https://github.com/utexas-bwi/kinova-ros.git 12 | version: melodic-devel 13 | - git: 14 | local-name: libsegwayrmp 15 | uri: https://github.com/utexas-bwi/libsegwayrmp.git 16 | version: cpp11 17 | - git: 18 | local-name: segway_rmp 19 | uri: https://github.com/utexas-bwi/segway_rmp.git 20 | version: melodic-devel 21 | - git: 22 | local-name: segway_v3 23 | uri: https://github.com/utexas-bwi/segway_v3.git 24 | version: melodic-devel 25 | - git: 26 | local-name: camera1394 27 | uri: https://github.com/ros-drivers/camera1394 28 | version: master 29 | - git: 30 | local-name: frontier_exploration 31 | uri: https://github.com/paulbovbel/frontier_exploration.git 32 | version: melodic-devel 33 | 34 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | dist: trusty 3 | language: generic 4 | compiler: 5 | - gcc 6 | env: 7 | global: 8 | - CATKIN_CONFIG='--no-install' # work around kinova-ros install bug 9 | - CCACHE_DIR=$HOME/.ccache 10 | - ROS_DISTRO="melodic" 11 | - ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu 12 | - UPSTREAM_WORKSPACE=file 13 | matrix: 14 | - BUILD_PKGS_WHITELIST="only_segbot_base" 15 | #- BUILD_PKGS_WHITELIST="only_segbot_arm" 16 | install: 17 | - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config 18 | script: 19 | - source .ci_config/travis.sh 20 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2014, The University of Texas at Austin 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | * Redistributions of source code must retain the above copyright 7 | notice, this list of conditions and the following disclaimer. 8 | * Redistributions in binary form must reproduce the above copyright 9 | notice, this list of conditions and the following disclaimer in the 10 | documentation and/or other materials provided with the distribution. 11 | * Neither the name of the nor the 12 | names of its contributors may be used to endorse or promote products 13 | derived from this software without specific prior written permission. 14 | * All academic uses of this work should cite the following paper: 15 | "Piyush Khandelwal, Fangkai Yang, Matteo Leonetti, Vladimir Lifschitz, 16 | and Peter Stone. Planning in Action Language BC while Learning Action 17 | Costs for Mobile Robots. International Conference on Automated 18 | Planning and Scheduling (ICAPS). 2014." 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 21 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 22 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 24 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 25 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 26 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 27 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | The segbot ROS metapackage 2 | ========================== 3 | 4 | [![Build Status](https://travis-ci.org/utexas-bwi/segbot.svg?branch=master)](https://travis-ci.org/utexas-bwi/segbot) 5 | 6 | ROS drivers for controlling Segway-based robots at the Learning Agents 7 | Research Group (LARG), AI Laboratory, Department of Computer Science, 8 | University of Texas at Austin. 9 | 10 | Documentation for much of this code-base can be found on the ROS wiki: 11 | * [segbot](http://wiki.ros.org/segbot) 12 | 13 | All the code in this package has been released using a modified BSD license, 14 | which can be found with this package [here](LICENSE). 15 | 16 | All academic uses of this work should cite the following representative paper: 17 | "Piyush Khandelwal, Fangkai Yang, Matteo Leonetti, Vladimir Lifschitz, and 18 | Peter Stone. Planning in Action Language BC while Learning Action Costs for 19 | Mobile Robots. International Conference on Automated Planning and Scheduling 20 | (ICAPS). 2014." 21 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/.setup_assistant: -------------------------------------------------------------------------------- 1 | moveit_setup_assistant_config: 2 | URDF: 3 | package: kinova_description 4 | relative_path: urdf/m1n6s300_standalone.xacro 5 | SRDF: 6 | relative_path: config/m1n6s300.srdf 7 | CONFIG: 8 | generated_timestamp: 1484110350 -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(m1n6s200_segbot_moveit_config) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 9 | install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 10 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/config/controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: m1n6s200 3 | action_ns: follow_joint_trajectory 4 | type: FollowJointTrajectory 5 | default: true 6 | joints: 7 | - m1n6s200_joint_1 8 | - m1n6s200_joint_2 9 | - m1n6s200_joint_3 10 | - m1n6s200_joint_4 11 | - m1n6s200_joint_5 12 | - m1n6s200_joint_6 13 | - name: m1n6s200_gripper 14 | action_ns: gripper_command 15 | type: GripperCommand 16 | default: true 17 | joints: 18 | - m1n6s200_joint_finger_1 19 | - m1n6s200_joint_finger_2 20 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/config/controllers_ros_control.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: m1n6s200/effort_joint_trajectory_controller 3 | action_ns: follow_joint_trajectory 4 | type: FollowJointTrajectory 5 | default: true 6 | joints: 7 | - m1n6s200_joint_1 8 | - m1n6s200_joint_2 9 | - m1n6s200_joint_3 10 | - m1n6s200_joint_4 11 | - m1n6s200_joint_5 12 | - m1n6s200_joint_6 13 | - name: m1n6s200/effort_finger_trajectory_controller 14 | action_ns: follow_joint_trajectory 15 | type: FollowJointTrajectory 16 | default: true 17 | joints: 18 | - m1n6s200_joint_finger_1 19 | - m1n6s200_joint_finger_2 20 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/config/fake_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: fake_arm_controller 3 | joints: 4 | - m1n6s200_joint_1 5 | - m1n6s200_joint_2 6 | - m1n6s200_joint_3 7 | - m1n6s200_joint_4 8 | - m1n6s200_joint_5 9 | - m1n6s200_joint_6 10 | - name: fake_gripper_controller 11 | joints: 12 | - m1n6s200_joint_finger_1 13 | - m1n6s200_joint_finger_2 14 | 15 | initial: 16 | - group: arm 17 | pose: Home 18 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 3 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 4 | joint_limits: 5 | m1n6s200_joint_1: 6 | has_velocity_limits: true 7 | max_velocity: 0.35 8 | has_acceleration_limits: false 9 | max_acceleration: 0 10 | m1n6s200_joint_2: 11 | has_velocity_limits: true 12 | max_velocity: 0.35 13 | has_acceleration_limits: false 14 | max_acceleration: 0 15 | m1n6s200_joint_3: 16 | has_velocity_limits: true 17 | max_velocity: 0.35 18 | has_acceleration_limits: false 19 | max_acceleration: 0 20 | m1n6s200_joint_4: 21 | has_velocity_limits: true 22 | max_velocity: 0.35 23 | has_acceleration_limits: false 24 | max_acceleration: 0 25 | m1n6s200_joint_5: 26 | has_velocity_limits: true 27 | max_velocity: 0.35 28 | has_acceleration_limits: false 29 | max_acceleration: 0 30 | m1n6s200_joint_6: 31 | has_velocity_limits: true 32 | max_velocity: 0.35 33 | has_acceleration_limits: false 34 | max_acceleration: 0 35 | m1n6s200_joint_finger_1: 36 | has_velocity_limits: true 37 | max_velocity: 5 38 | has_acceleration_limits: false 39 | max_acceleration: 0 40 | m1n6s200_joint_finger_2: 41 | has_velocity_limits: true 42 | max_velocity: 5 43 | has_acceleration_limits: false 44 | max_acceleration: 0 45 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/config/joint_names.yaml: -------------------------------------------------------------------------------- 1 | controller_joint_names: [m1n6s200_joint_1 m1n6s200_joint_2 m1n6s200_joint_3 m1n6s200_joint_4 m1n6s200_joint_5 m1n6s200_joint_6] 2 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | arm: 2 | kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin 3 | #kinematics_solver: m1n6s200_mico_arm_kinematics/IKFastKinematicsPlugin 4 | #kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 5 | kinematics_solver_attempts: 3 6 | kinematics_solver_search_resolution: 0.005 7 | kinematics_solver_timeout: 0.005 8 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/launch/default_warehouse_db.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/launch/fake_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/launch/joystick_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/launch/m1n6s200_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/launch/m1n6s200_moveit_sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/launch/m1n6s200_ros_control_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/launch/moveit_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/launch/ompl_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/launch/planning_context.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/launch/planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/launch/run_benchmark_ompl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/launch/sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/launch/setup_assistant.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/launch/trajectory_execution.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/launch/warehouse.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/launch/warehouse_settings.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /m1n6s200_segbot_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | m1n6s200_segbot_moveit_config 4 | 0.2.0 5 | 6 | An automatically generated package with all the configuration and launch files for using the m1n6s300 with the MoveIt Motion Planning Framework 7 | 8 | MoveIt Setup Assistant 9 | MoveIt Setup Assistant 10 | 11 | BSD 12 | 13 | http://moveit.ros.org/ 14 | https://github.com/ros-planning/moveit_setup_assistant/issues 15 | https://github.com/ros-planning/moveit_setup_assistant 16 | 17 | moveit_ros_move_group 18 | moveit_planners_ompl 19 | moveit_ros_visualization 20 | joint_state_publisher 21 | robot_state_publisher 22 | xacro 23 | kinova_description 24 | kinova_description 25 | trac_ik 26 | 27 | 28 | catkin 29 | 30 | 31 | -------------------------------------------------------------------------------- /only_segbot_arm/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package only_segbot_arm 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | forthcoming 6 | ----------- 7 | * New metapackage for building only the segbot arm packages. 8 | 9 | 0.3.5 (2016-08-27) 10 | ------------------ 11 | -------------------------------------------------------------------------------- /only_segbot_arm/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(only_segbot_arm) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /only_segbot_arm/package.xml: -------------------------------------------------------------------------------- 1 | 2 | only_segbot_arm 3 | 0.3.5 4 | 5 | ROS packages for controlling manipulator arms on various Segway 6 | robot platforms maintained by the Learning Agents Research Group 7 | (LARG), AI Laboratory, Department of Computer Science, University 8 | of Texas at Austin. 9 | 10 | 11 | BSD 12 | 13 | https://github.com/utexas-bwi/segbot.git 14 | https://github.com/utexas-bwi/segbot/issues 15 | 16 | Jack O'Quin 17 | Jack O'Quin 18 | 19 | catkin 20 | 21 | segbot_arm_joystick 22 | segbot_arm_manipulation 23 | segbot_arm_tasks 24 | segbot_arm_tutorials 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /only_segbot_base/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package only_segbot_base 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | forthcoming 6 | ----------- 7 | * New metapackage for building only the segbot base packages. 8 | 9 | 0.3.5 (2016-08-27) 10 | ------------------ 11 | -------------------------------------------------------------------------------- /only_segbot_base/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(only_segbot_base) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /only_segbot_base/package.xml: -------------------------------------------------------------------------------- 1 | 2 | only_segbot_base 3 | 0.3.5 4 | 5 | ROS packages for controlling the bases of various Segway robot 6 | platforms maintained by the Learning Agents Research Group (LARG), 7 | AI Laboratory, Department of Computer Science, University of Texas 8 | at Austin. 9 | 10 | 11 | BSD 12 | 13 | https://github.com/utexas-bwi/segbot.git 14 | https://github.com/utexas-bwi/segbot/issues 15 | 16 | Jack O'Quin 17 | Jack O'Quin 18 | 19 | catkin 20 | 21 | segbot_bringup 22 | segbot_description 23 | segbot_firmware 24 | segbot_gazebo 25 | segbot_led 26 | segbot_navigation 27 | segbot_sensors 28 | segbot_simulation_apps 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /segbot/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package segbot 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2016-08-27) 6 | ------------------ 7 | 8 | 0.3.4 (2016-08-08) 9 | ------------------ 10 | * segbot: update metapackage description 11 | * Contributors: Jack O'Quin 12 | 13 | 0.3.3 (2015-08-05) 14 | ------------------ 15 | * merge segbot_simulator packages into segbot (`#46 `_) 16 | * merge segbot_apps packages into segbot (`#46 `_) 17 | * Contributors: Jack O'Quin 18 | 19 | 0.3.2 (2015-03-31) 20 | ------------------ 21 | 22 | 0.3.1 (2015-03-24) 23 | ------------------ 24 | 25 | 0.3.0 (2015-03-14) 26 | ------------------ 27 | * added platform independent tag to metapackage. 28 | * Contributors: Piyush Khandelwal 29 | 30 | 0.2.1 (2014-04-24) 31 | ------------------ 32 | 33 | 0.2.0 (2014-04-17) 34 | ------------------ 35 | 36 | * Release to Hydro. 37 | * Segbot_sensors: add sensor_ranges driver for Arduino data. 38 | * Updated descriptions and licensing information. 39 | 40 | 0.1.9 (2013-12-15) 41 | ------------------ 42 | 43 | 0.1.8 (2013-12-04) 44 | ------------------ 45 | 46 | 0.1.7 (2013-09-03) 47 | ------------------ 48 | 49 | 0.1.6 (2013-08-13) 50 | ------------------ 51 | 52 | 0.1.5 (2013-07-16) 53 | ------------------ 54 | * cleaned up and alphabetized cmake and package description files 55 | 56 | 0.1.4 (2013-07-13) 57 | ------------------ 58 | * releasing 0.1.4 with properly formatted changelogs. see `#10 `_ 59 | 60 | 0.1.3 (2013-07-10) 61 | ------------------ 62 | * bumping metapackage version 63 | 64 | 0.1.0 (2013-06-28) 65 | ------------------ 66 | * catkinizing against hydro. progress towards `#6 `_ 67 | -------------------------------------------------------------------------------- /segbot/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(segbot) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /segbot/package.xml: -------------------------------------------------------------------------------- 1 | 2 | segbot 3 | 0.3.5 4 | 5 | ROS packages for controlling Segway-based robots at the Learning Agents 6 | Research Group (LARG), AI Laboratory, Department of Computer Science, 7 | University of Texas at Austin. 8 | 9 | 10 | BSD 11 | 12 | http://ros.org/wiki/segbot 13 | https://github.com/utexas-bwi/segbot.git 14 | https://github.com/utexas-bwi/segbot/issues 15 | 16 | Piyush Khandelwal 17 | Piyush Khandelwal 18 | Jack O'Quin 19 | 20 | catkin 21 | 22 | only_segbot_arm 23 | only_segbot_base 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8 FATAL_ERROR) 2 | project(draw_demo) 3 | 4 | if( $ENV{SEGBOT_HAS_ARM} ) 5 | find_package(catkin REQUIRED COMPONENTS 6 | actionlib 7 | geometry_msgs 8 | image_transport 9 | kinova_msgs 10 | message_generation 11 | bwi_moveit_utils 12 | pcl_ros 13 | pcl_conversions 14 | roscpp 15 | bwi_perception 16 | sensor_msgs 17 | std_msgs 18 | ) 19 | find_package(OpenCV REQUIRED) 20 | 21 | catkin_package( 22 | LIBRARIES kinova_msgs 23 | CATKIN_DEPENDS kinova_msgs #pcl_conversions pcl_ros roscpp sensor_msgs 24 | ) 25 | 26 | 27 | include_directories( 28 | ${catkin_INCLUDE_DIRS} 29 | ${openCV_INCLUDE_DIRS} 30 | ) 31 | 32 | add_executable (recordTrajectories src/recordTrajectory.cpp) 33 | target_link_libraries(recordTrajectories ${catkin_LIBRARIES}) 34 | 35 | add_executable (visionDistance src/visionDistance.cpp) 36 | target_link_libraries(visionDistance ${catkin_LIBRARIES} ${OpenCV_LIBS}) 37 | 38 | 39 | #fingers open/close executable 40 | add_executable (fingersWorkout src/moveFingers.cpp) 41 | target_link_libraries(fingersWorkout ${catkin_LIBRARIES}) 42 | #recordpoints exe 43 | add_executable (recordPoints src/recordPoints.cpp) 44 | target_link_libraries(recordPoints ${catkin_LIBRARIES}) 45 | 46 | #shapes exe 47 | add_executable (drawShapes src/shapes.cpp) 48 | target_link_libraries(drawShapes ${catkin_LIBRARIES}) 49 | 50 | #draw exe 51 | add_executable (draw src/draw.cpp) 52 | target_link_libraries(draw ${catkin_LIBRARIES}) 53 | 54 | #approach board 55 | add_executable(approach_board src/approach_board.cpp) 56 | target_link_libraries(approach_board ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 57 | 58 | #movement_demos exe 59 | add_executable (movement_demo src/movement_demo.cpp) 60 | target_link_libraries(movement_demo ${catkin_LIBRARIES}) 61 | 62 | endif() 63 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/config/common_angles.txt: -------------------------------------------------------------------------------- 1 | hopefullythispointwillbeinconfigfolder, 1 1 1 1 1 1 2 | hopefullythisdoesntwverasdfasdride, 1 1 1 1 1 1 3 | this, -1.67956 -1.4322 -0.335912 -1.10075 -4.85162 3.58586 4 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/launch/approach_board.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | /> 4 | 5 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project( transforms ) 3 | find_package( OpenCV REQUIRED ) 4 | 5 | 6 | add_executable( transforms htransformp.cpp ) 7 | target_link_libraries( transforms ${OpenCV_LIBS} ) 8 | 9 | add_executable( findContours findContours.cpp ) 10 | target_link_libraries( findContours ${OpenCV_LIBS} ) 11 | 12 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/IMG_5465-1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_arm_demos/draw_demo/opencv/IMG_5465-1.jpg -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/barn.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0436562 Y:0.0672042 X:0.0447146 Y:0.0552979 X:0.0584729 Y:0.0550333 X:0.0587375 Y:0.0687917 X:0.0590021 Y:0.0542396 2 | P2P X:0.0494771 Y:0.0333375 X:0.047625 Y:0.0415396 X:0.0590021 Y:0.0433917 X:0.0592667 Y:0.0341312 X:0.0505354 Y:0.0333375 3 | P2P X:0.0195792 Y:0.0015875 X:0.00608542 Y:0.00820208 X:0.00449792 Y:0.01905 X:0.0103187 Y:0.0320146 X:0.0296333 Y:0.0661458 X:0.0322792 Y:0.0388937 X:0.0478896 Y:0.0275167 X:0.0563562 Y:0.0259292 X:0.0727604 Y:0.0388937 X:0.0738187 Y:0.0558271 X:0.0738187 Y:0.0701146 X:0.0746125 Y:0.0558271 X:0.0748771 Y:0.0396875 X:0.0550333 Y:0.0240771 X:0.0473604 Y:0.0185208 X:0.0306917 Y:0.0079375 X:0.0227542 Y:0.00291042 4 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/barn.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_arm_demos/draw_demo/opencv/barn.png -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/barn2.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0510646 Y:0.0574146 X:0.0510646 Y:0.0717021 X:0.0518583 Y:0.0574146 X:0.0642937 Y:0.0568854 X:0.0642937 Y:0.0711729 X:0.0648229 Y:0.0560917 2 | P2P X:0.0526521 Y:0.0338667 X:0.0505354 Y:0.0423333 X:0.0650875 Y:0.0431271 X:0.0650875 Y:0.0341312 3 | P2P X:0.0216958 Y:0.00978958 X:0.009525 Y:0.0161396 X:0.00820208 Y:0.0259292 X:0.00846667 Y:0.0365125 X:0.0132292 Y:0.0436562 X:0.0214312 Y:0.0547687 X:0.0298979 Y:0.0658812 X:0.0346604 Y:0.0724958 X:0.0354542 Y:0.0555625 X:0.0359833 Y:0.0375708 X:0.0439208 Y:0.0312208 X:0.0529167 Y:0.0240771 X:0.0613833 Y:0.0277812 X:0.0727604 Y:0.0367771 X:0.0754062 Y:0.0563562 X:0.0754062 Y:0.0719667 X:0.0762 Y:0.0563562 X:0.0764646 Y:0.0388937 X:0.0685271 Y:0.0322792 X:0.0574146 Y:0.0232833 X:0.0486833 Y:0.01905 X:0.0227542 Y:0.00978958 4 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/barn2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_arm_demos/draw_demo/opencv/barn2.png -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/building.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_arm_demos/draw_demo/opencv/building.jpg -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/bwi.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_arm_demos/draw_demo/opencv/bwi.png -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/bwi2.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0711729 Y:0.0661458 X:0.0751417 Y:0.0735542 X:0.0841375 Y:0.0735542 X:0.0841375 Y:0.0627062 2 | P2P X:0.0788458 Y:0.0497417 X:0.0724958 Y:0.0560917 X:0.0841375 Y:0.0590021 X:0.0841375 Y:0.0500062 3 | P2P X:0.0748771 Y:0.0465667 X:0.0682625 Y:0.0521229 X:0.0706437 Y:0.0597958 X:0.0682625 Y:0.0521229 X:0.0748771 Y:0.0465667 X:0.0878417 Y:0.0463021 X:0.0881062 Y:0.0759354 X:0.0714375 Y:0.0759354 X:0.0669396 Y:0.0664104 X:0.0727604 Y:0.0608542 X:0.0669396 Y:0.0664104 X:0.0693208 Y:0.0748771 X:0.0883708 Y:0.0759354 X:0.0883708 Y:0.0465667 4 | P2P X:0.0187854 Y:0.0463021 X:0.0201083 Y:0.0547687 X:0.0235479 Y:0.0685271 X:0.0314854 Y:0.0756708 X:0.0338667 Y:0.0661458 X:0.0365125 Y:0.0552979 X:0.0407458 Y:0.0629708 X:0.0433917 Y:0.0746125 X:0.0513292 Y:0.0695854 X:0.0568854 Y:0.0473604 X:0.0508 Y:0.0542396 X:0.0468312 Y:0.0701146 X:0.0404812 Y:0.0484187 X:0.0330729 Y:0.0547687 X:0.0291042 Y:0.0706437 X:0.0246062 Y:0.0550333 X:0.022225 Y:0.0465667 5 | P2P X:0.00238125 Y:0.0463021 X:0.0015875 Y:0.0754062 X:0.00582083 Y:0.0465667 6 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/circle.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0370417 Y:0.0248708 X:0.0291042 Y:0.0296333 X:0.0230187 Y:0.0373062 X:0.0203729 Y:0.0449792 X:0.0201083 Y:0.0534458 X:0.0224896 Y:0.0616479 X:0.0296333 Y:0.0709083 X:0.0375708 Y:0.0754062 X:0.0492125 Y:0.0769937 X:0.0582083 Y:0.0746125 X:0.0674687 Y:0.0677333 X:0.0722312 Y:0.0595312 X:0.0738187 Y:0.0470958 X:0.0714375 Y:0.0386292 X:0.066675 Y:0.03175 X:0.0597958 Y:0.0261937 X:0.0521229 Y:0.0235479 2 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/circle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_arm_demos/draw_demo/opencv/circle.png -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/cmake_install.cmake: -------------------------------------------------------------------------------- 1 | # Install script for directory: /home/maxwell/git_ws/src/bwi_experimental/robot_arm/opencv 2 | 3 | # Set the install prefix 4 | IF(NOT DEFINED CMAKE_INSTALL_PREFIX) 5 | SET(CMAKE_INSTALL_PREFIX "/usr/local") 6 | ENDIF(NOT DEFINED CMAKE_INSTALL_PREFIX) 7 | STRING(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}") 8 | 9 | # Set the install configuration name. 10 | IF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) 11 | IF(BUILD_TYPE) 12 | STRING(REGEX REPLACE "^[^A-Za-z0-9_]+" "" 13 | CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}") 14 | ELSE(BUILD_TYPE) 15 | SET(CMAKE_INSTALL_CONFIG_NAME "") 16 | ENDIF(BUILD_TYPE) 17 | MESSAGE(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"") 18 | ENDIF(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) 19 | 20 | # Set the component getting installed. 21 | IF(NOT CMAKE_INSTALL_COMPONENT) 22 | IF(COMPONENT) 23 | MESSAGE(STATUS "Install component: \"${COMPONENT}\"") 24 | SET(CMAKE_INSTALL_COMPONENT "${COMPONENT}") 25 | ELSE(COMPONENT) 26 | SET(CMAKE_INSTALL_COMPONENT) 27 | ENDIF(COMPONENT) 28 | ENDIF(NOT CMAKE_INSTALL_COMPONENT) 29 | 30 | # Install shared libraries without execute permission? 31 | IF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) 32 | SET(CMAKE_INSTALL_SO_NO_EXE "1") 33 | ENDIF(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) 34 | 35 | IF(CMAKE_INSTALL_COMPONENT) 36 | SET(CMAKE_INSTALL_MANIFEST "install_manifest_${CMAKE_INSTALL_COMPONENT}.txt") 37 | ELSE(CMAKE_INSTALL_COMPONENT) 38 | SET(CMAKE_INSTALL_MANIFEST "install_manifest.txt") 39 | ENDIF(CMAKE_INSTALL_COMPONENT) 40 | 41 | FILE(WRITE "/home/maxwell/git_ws/src/bwi_experimental/robot_arm/opencv/${CMAKE_INSTALL_MANIFEST}" "") 42 | FOREACH(file ${CMAKE_INSTALL_MANIFEST_FILES}) 43 | FILE(APPEND "/home/maxwell/git_ws/src/bwi_experimental/robot_arm/opencv/${CMAKE_INSTALL_MANIFEST}" "${file}\n") 44 | ENDFOREACH(file) 45 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/cube.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0174625 Y:0.015875 X:0.0103187 Y:0.0211667 X:0.0171979 Y:0.0169333 X:0.0256646 Y:0.0105833 X:0.0330729 Y:0.00449792 X:0.0867833 Y:0.0047625 X:0.0682625 Y:0.0230187 X:0.00978958 Y:0.0232833 X:0.0679979 Y:0.0240771 X:0.0679979 Y:0.0431271 X:0.0674687 Y:0.0682625 X:0.0439208 Y:0.0738187 X:0.0142875 Y:0.0740833 X:0.00873125 Y:0.0235479 X:0.00820208 Y:0.0748771 X:0.0439208 Y:0.0746125 X:0.0672042 Y:0.0743479 X:0.0722312 Y:0.0674687 X:0.0806979 Y:0.0550333 X:0.085725 Y:0.047625 X:0.0873125 Y:0.0209021 X:0.0875771 Y:0.00502708 2 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/cube.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_arm_demos/draw_demo/opencv/cube.png -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/cube2.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0116417 Y:0.0209021 X:0.0116417 Y:0.0449792 X:0.0182562 Y:0.0550333 X:0.0267229 Y:0.0674687 X:0.0314854 Y:0.0740833 X:0.0550333 Y:0.0746125 X:0.0849312 Y:0.0748771 X:0.0907521 Y:0.0235479 X:0.0899583 Y:0.0740833 X:0.0550333 Y:0.0738187 X:0.0320146 Y:0.0735542 X:0.0309562 Y:0.0431271 X:0.0312208 Y:0.0238125 X:0.0891646 Y:0.0238125 X:0.0306917 Y:0.0230187 X:0.0169333 Y:0.009525 X:0.0658812 Y:0.00449792 X:0.0732896 Y:0.0105833 X:0.0817562 Y:0.0169333 X:0.0886354 Y:0.0224896 X:0.0814917 Y:0.015875 X:0.0682625 Y:0.00529167 X:0.0116417 Y:0.00396875 2 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/drawing_files/barn.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0510646 Y:0.0574146 X:0.0510646 Y:0.0717021 X:0.0518583 Y:0.0574146 X:0.0642937 Y:0.0568854 X:0.0642937 Y:0.0711729 X:0.0648229 Y:0.0560917 2 | P2P X:0.0526521 Y:0.0338667 X:0.0505354 Y:0.0423333 X:0.0650875 Y:0.0431271 X:0.0650875 Y:0.0341312 3 | P2P X:0.0216958 Y:0.00978958 X:0.009525 Y:0.0161396 X:0.00820208 Y:0.0259292 X:0.00846667 Y:0.0365125 X:0.0132292 Y:0.0436562 X:0.0214312 Y:0.0547687 X:0.0298979 Y:0.0658812 X:0.0346604 Y:0.0724958 X:0.0354542 Y:0.0555625 X:0.0359833 Y:0.0375708 X:0.0439208 Y:0.0312208 X:0.0529167 Y:0.0240771 X:0.0613833 Y:0.0277812 X:0.0727604 Y:0.0367771 X:0.0754062 Y:0.0563562 X:0.0754062 Y:0.0719667 X:0.0762 Y:0.0563562 X:0.0764646 Y:0.0388937 X:0.0685271 Y:0.0322792 X:0.0574146 Y:0.0232833 X:0.0486833 Y:0.01905 X:0.0227542 Y:0.00978958 4 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/drawing_files/barn2.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0436562 Y:0.0672042 X:0.0447146 Y:0.0552979 X:0.0584729 Y:0.0550333 X:0.0587375 Y:0.0687917 X:0.0590021 Y:0.0542396 2 | P2P X:0.0494771 Y:0.0333375 X:0.047625 Y:0.0415396 X:0.0590021 Y:0.0433917 X:0.0592667 Y:0.0341312 X:0.0505354 Y:0.0333375 3 | P2P X:0.0195792 Y:0.0015875 X:0.00608542 Y:0.00820208 X:0.00449792 Y:0.01905 X:0.0103187 Y:0.0320146 X:0.0296333 Y:0.0661458 X:0.0322792 Y:0.0388937 X:0.0478896 Y:0.0275167 X:0.0563562 Y:0.0259292 X:0.0727604 Y:0.0388937 X:0.0738187 Y:0.0558271 X:0.0738187 Y:0.0701146 X:0.0746125 Y:0.0558271 X:0.0748771 Y:0.0396875 X:0.0550333 Y:0.0240771 X:0.0473604 Y:0.0185208 X:0.0306917 Y:0.0079375 X:0.0227542 Y:0.00291042 4 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/drawing_files/bwi.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0079375 Y:0.073025 X:0.0171979 Y:0.0735542 X:0.0211667 Y:0.0661458 X:0.0140229 Y:0.0624417 2 | P2P X:0.00873125 Y:0.0497417 X:0.0079375 Y:0.0584729 X:0.0164042 Y:0.0590021 X:0.01905 Y:0.0513292 3 | P2P X:0.0870479 Y:0.0463021 X:0.0862542 Y:0.0754062 X:0.0904875 Y:0.0465667 4 | P2P X:0.0359833 Y:0.0463021 X:0.0373062 Y:0.0547687 X:0.0423333 Y:0.0748771 X:0.0513292 Y:0.0650875 X:0.0545042 Y:0.0521229 X:0.05715 Y:0.0605896 X:0.0605896 Y:0.0746125 X:0.0685271 Y:0.0685271 X:0.0740833 Y:0.0473604 X:0.0672042 Y:0.05715 X:0.0640292 Y:0.0701146 X:0.0576792 Y:0.0484187 X:0.0502708 Y:0.0547687 X:0.0463021 Y:0.0706437 X:0.0399521 Y:0.0478896 5 | P2P X:0.00449792 Y:0.0463021 X:0.00370417 Y:0.0754062 X:0.0211667 Y:0.0759354 X:0.0254 Y:0.0664104 X:0.0195792 Y:0.0608542 X:0.0254 Y:0.0664104 X:0.0227542 Y:0.0748771 X:0.00423333 Y:0.0759354 X:0.00370417 Y:0.0470958 X:0.0140229 Y:0.0463021 X:0.0216958 Y:0.0486833 X:0.0238125 Y:0.0568854 X:0.0224896 Y:0.0489479 X:0.0140229 Y:0.0463021 6 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/drawing_files/bwi2.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0711729 Y:0.0661458 X:0.0751417 Y:0.0735542 X:0.0841375 Y:0.0735542 X:0.0841375 Y:0.0627062 2 | P2P X:0.0788458 Y:0.0497417 X:0.0724958 Y:0.0560917 X:0.0841375 Y:0.0590021 X:0.0841375 Y:0.0500062 3 | P2P X:0.0748771 Y:0.0465667 X:0.0682625 Y:0.0521229 X:0.0706437 Y:0.0597958 X:0.0682625 Y:0.0521229 X:0.0748771 Y:0.0465667 X:0.0878417 Y:0.0463021 X:0.0881062 Y:0.0759354 X:0.0714375 Y:0.0759354 X:0.0669396 Y:0.0664104 X:0.0727604 Y:0.0608542 X:0.0669396 Y:0.0664104 X:0.0693208 Y:0.0748771 X:0.0883708 Y:0.0759354 X:0.0883708 Y:0.0465667 4 | P2P X:0.0187854 Y:0.0463021 X:0.0201083 Y:0.0547687 X:0.0235479 Y:0.0685271 X:0.0314854 Y:0.0756708 X:0.0338667 Y:0.0661458 X:0.0365125 Y:0.0552979 X:0.0407458 Y:0.0629708 X:0.0433917 Y:0.0746125 X:0.0513292 Y:0.0695854 X:0.0568854 Y:0.0473604 X:0.0508 Y:0.0542396 X:0.0468312 Y:0.0701146 X:0.0404812 Y:0.0484187 X:0.0330729 Y:0.0547687 X:0.0291042 Y:0.0706437 X:0.0246062 Y:0.0550333 X:0.022225 Y:0.0465667 5 | P2P X:0.00238125 Y:0.0463021 X:0.0015875 Y:0.0754062 X:0.00582083 Y:0.0465667 6 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/drawing_files/bwi3.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0587375 Y:0.0296333 X:0.0605896 Y:0.0394229 X:0.0558271 Y:0.0486833 X:0.0613833 Y:0.0550333 X:0.0754062 Y:0.0555625 X:0.0756708 Y:0.0407458 X:0.0738187 Y:0.0534458 X:0.0642937 Y:0.053975 X:0.0576792 Y:0.0486833 X:0.0608542 Y:0.041275 X:0.0724958 Y:0.0396875 X:0.0635 Y:0.0381 X:0.060325 Y:0.0306917 X:0.0732896 Y:0.0277812 X:0.0740833 Y:0.0381 X:0.0754062 Y:0.0261937 2 | P2P X:0.0513292 Y:0.0261937 X:0.0468312 Y:0.0367771 X:0.0431271 Y:0.0478896 X:0.0365125 Y:0.0341312 X:0.0338667 Y:0.0261937 X:0.0365125 Y:0.0341312 X:0.0396875 Y:0.0433917 X:0.0494771 Y:0.0293687 X:0.0463021 Y:0.0439208 X:0.0423333 Y:0.0550333 X:0.034925 Y:0.0359833 X:0.0415396 Y:0.0550333 X:0.0505354 Y:0.0320146 3 | P2P X:0.0150812 Y:0.0259292 X:0.01905 Y:0.0388937 X:0.0243417 Y:0.053975 X:0.0301625 Y:0.041275 X:0.0330729 Y:0.0325437 X:0.0301625 Y:0.041275 X:0.0267229 Y:0.0513292 X:0.0169333 Y:0.0336021 X:0.0246062 Y:0.0492125 X:0.0291042 Y:0.0381 X:0.0325437 Y:0.0288396 X:0.0259292 Y:0.0473604 X:0.0177271 Y:0.0304271 4 | P2P X:0.00661458 Y:0.0261937 X:0.00661458 Y:0.0555625 X:0.00820208 Y:0.0261937 5 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/drawing_files/circle.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0370417 Y:0.0248708 X:0.0291042 Y:0.0296333 X:0.0230187 Y:0.0373062 X:0.0203729 Y:0.0449792 X:0.0201083 Y:0.0534458 X:0.0224896 Y:0.0616479 X:0.0296333 Y:0.0709083 X:0.0375708 Y:0.0754062 X:0.0492125 Y:0.0769937 X:0.0582083 Y:0.0746125 X:0.0674687 Y:0.0677333 X:0.0722312 Y:0.0595312 X:0.0738187 Y:0.0470958 X:0.0714375 Y:0.0386292 X:0.066675 Y:0.03175 X:0.0597958 Y:0.0261937 X:0.0521229 Y:0.0235479 2 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/drawing_files/cube.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0174625 Y:0.015875 X:0.0103187 Y:0.0211667 X:0.0171979 Y:0.0169333 X:0.0256646 Y:0.0105833 X:0.0330729 Y:0.00449792 X:0.0867833 Y:0.0047625 X:0.0682625 Y:0.0230187 X:0.00978958 Y:0.0232833 X:0.0679979 Y:0.0240771 X:0.0679979 Y:0.0431271 X:0.0674687 Y:0.0682625 X:0.0439208 Y:0.0738187 X:0.0142875 Y:0.0740833 X:0.00873125 Y:0.0235479 X:0.00820208 Y:0.0748771 X:0.0439208 Y:0.0746125 X:0.0672042 Y:0.0743479 X:0.0722312 Y:0.0674687 X:0.0806979 Y:0.0550333 X:0.085725 Y:0.047625 X:0.0873125 Y:0.0209021 X:0.0875771 Y:0.00502708 2 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/drawing_files/cube2.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0116417 Y:0.0209021 X:0.0116417 Y:0.0449792 X:0.0182562 Y:0.0550333 X:0.0267229 Y:0.0674687 X:0.0314854 Y:0.0740833 X:0.0550333 Y:0.0746125 X:0.0849312 Y:0.0748771 X:0.0907521 Y:0.0235479 X:0.0899583 Y:0.0740833 X:0.0550333 Y:0.0738187 X:0.0320146 Y:0.0735542 X:0.0309562 Y:0.0431271 X:0.0312208 Y:0.0238125 X:0.0891646 Y:0.0238125 X:0.0306917 Y:0.0230187 X:0.0169333 Y:0.009525 X:0.0658812 Y:0.00449792 X:0.0732896 Y:0.0105833 X:0.0817562 Y:0.0169333 X:0.0886354 Y:0.0224896 X:0.0814917 Y:0.015875 X:0.0682625 Y:0.00529167 X:0.0116417 Y:0.00396875 2 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/drawing_files/house.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0531812 Y:0.0473604 X:0.0518583 Y:0.060325 X:0.0521229 Y:0.0724958 X:0.0526521 Y:0.060325 X:0.0529167 Y:0.0484187 X:0.0613833 Y:0.047625 X:0.0682625 Y:0.0722312 X:0.0685271 Y:0.0470958 2 | P2P X:0.0518583 Y:0.0145521 X:0.0510646 Y:0.0256646 X:0.0674687 Y:0.0288396 X:0.0679979 Y:0.0148167 X:0.0560917 Y:0.0142875 3 | P2P X:0.0566208 Y:0.00502708 X:0.0497417 Y:0.00978958 X:0.0383646 Y:0.0182562 X:0.0309562 Y:0.0235479 X:0.0301625 Y:0.0717021 X:0.0309562 Y:0.0246062 X:0.041275 Y:0.0169333 X:0.0515937 Y:0.00978958 X:0.0619125 Y:0.00899583 X:0.0719667 Y:0.0153458 X:0.0844021 Y:0.0235479 X:0.0886354 Y:0.0727604 X:0.0891646 Y:0.0259292 X:0.0775229 Y:0.0179917 X:0.0621771 Y:0.0079375 4 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/drawing_files/longhorn.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0896937 Y:0.0288396 X:0.0820208 Y:0.0328083 X:0.0732896 Y:0.0394229 X:0.0711729 Y:0.0473604 X:0.0611187 Y:0.0473604 X:0.0566208 Y:0.0576792 X:0.0566208 Y:0.0672042 X:0.0518583 Y:0.0751417 X:0.0433917 Y:0.0738187 X:0.0420687 Y:0.0650875 X:0.0407458 Y:0.05715 X:0.0370417 Y:0.0492125 X:0.0283104 Y:0.0486833 X:0.0259292 Y:0.0404812 X:0.0177271 Y:0.0338667 X:0.0103187 Y:0.0296333 X:0.0015875 Y:0.028575 X:0.0103187 Y:0.0296333 X:0.0174625 Y:0.0338667 X:0.0248708 Y:0.0399521 X:0.0264583 Y:0.0478896 X:0.0346604 Y:0.0484187 X:0.0407458 Y:0.0574146 X:0.0415396 Y:0.0661458 X:0.0441854 Y:0.0746125 X:0.0526521 Y:0.0748771 X:0.0566208 Y:0.0672042 X:0.0563562 Y:0.0584729 X:0.060325 Y:0.0497417 X:0.0685271 Y:0.0486833 X:0.0722312 Y:0.0402167 X:0.079375 Y:0.034925 X:0.0862542 Y:0.0304271 X:0.0976312 Y:0.0280458 2 | P2P X:0.0902229 Y:0.0254 X:0.0820208 Y:0.0280458 X:0.0743479 Y:0.0322792 X:0.066675 Y:0.0367771 X:0.0584729 Y:0.0370417 X:0.0505354 Y:0.0357187 X:0.0407458 Y:0.0359833 X:0.03175 Y:0.0373062 X:0.0238125 Y:0.0333375 X:0.0134937 Y:0.0269875 X:0.00343958 Y:0.0254 X:0.0127 Y:0.0291042 X:0.0232833 Y:0.0370417 X:0.0304271 Y:0.041275 X:0.0373062 Y:0.0455083 X:0.0415396 Y:0.0560917 X:0.0436562 Y:0.0642937 X:0.0439208 Y:0.0724958 X:0.0521229 Y:0.0735542 X:0.0542396 Y:0.0650875 X:0.0555625 Y:0.0568854 X:0.0590021 Y:0.0489479 X:0.0677333 Y:0.0473604 X:0.0706437 Y:0.0394229 X:0.0780521 Y:0.0343958 X:0.0849312 Y:0.0293687 X:0.0973667 Y:0.0264583 3 | P2P X:0.0886354 Y:0.0243417 X:0.0809625 Y:0.0269875 X:0.0690562 Y:0.0341312 X:0.0600604 Y:0.0359833 X:0.0510646 Y:0.0346604 X:0.0415396 Y:0.0343958 X:0.0333375 Y:0.0362479 X:0.0251354 Y:0.0325437 X:0.0177271 Y:0.0275167 X:0.00873125 Y:0.0243417 X:0.000264583 Y:0.0248708 X:0.00873125 Y:0.0243417 X:0.0174625 Y:0.0275167 X:0.0248708 Y:0.0325437 X:0.0333375 Y:0.0362479 X:0.0415396 Y:0.0343958 X:0.0510646 Y:0.0346604 X:0.0590021 Y:0.0357187 X:0.0672042 Y:0.0351896 X:0.0777875 Y:0.0288396 X:0.0865187 Y:0.0248708 X:0.0957792 Y:0.0238125 4 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/drawing_files/longhorn2.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0108479 Y:0.022225 X:0.0195792 Y:0.0288396 X:0.0254 Y:0.0343958 X:0.0267229 Y:0.0428625 X:0.0346604 Y:0.0455083 X:0.0396875 Y:0.0518583 X:0.0428625 Y:0.060325 X:0.0420687 Y:0.0693208 X:0.0449792 Y:0.0769937 X:0.053975 Y:0.0756708 X:0.0545042 Y:0.0645583 X:0.0566208 Y:0.0547687 X:0.0592667 Y:0.0470958 X:0.0677333 Y:0.0455083 X:0.06985 Y:0.0362479 X:0.0772583 Y:0.0296333 X:0.0836083 Y:0.0240771 X:0.0910167 Y:0.0209021 X:0.0828146 Y:0.0193146 X:0.0748771 Y:0.0243417 X:0.0682625 Y:0.0296333 X:0.060325 Y:0.0320146 X:0.0515937 Y:0.0304271 X:0.0433917 Y:0.0298979 X:0.034925 Y:0.0320146 X:0.0272521 Y:0.0280458 X:0.0206375 Y:0.0227542 X:0.0129646 Y:0.0187854 2 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/drawing_files/longhorn3.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0105833 Y:0.0187854 X:0.0193146 Y:0.0267229 X:0.0216958 Y:0.034925 X:0.0301625 Y:0.0351896 X:0.0336021 Y:0.0457729 X:0.0336021 Y:0.0545042 X:0.0367771 Y:0.0619125 X:0.0439208 Y:0.0579437 X:0.0436562 Y:0.0489479 X:0.0465667 Y:0.0410104 X:0.053975 Y:0.0365125 X:0.05715 Y:0.0280458 X:0.0650875 Y:0.0206375 X:0.0727604 Y:0.0166687 X:0.0642937 Y:0.0164042 X:0.0568854 Y:0.0219604 X:0.0497417 Y:0.0256646 X:0.0418042 Y:0.0243417 X:0.0330729 Y:0.0240771 X:0.0248708 Y:0.0246062 X:0.0177271 Y:0.01905 X:0.00873125 Y:0.0142875 2 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/drawing_files/max.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:.0835 Y:.0153 X:.0662 Y:-.0145 X:.062 Y:.0007 X:.05 Y:-.0117 X:.0424 Y:.0163 2 | P2P X:.0334 Y:.0153 X:.020 Y:-.0098 X:.0153 Y:.0153 3 | P2P X:.0146423 Y:.00870563 X:.0317423 Y:.0087056 4 | P2P X:-.01088 Y:.0153 X:.0088 Y:-.0098 5 | P2P X:.0088 Y:.0153 X:-.01088 Y:-.0096 6 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/drawing_files/maxwell.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.190235 Y:0.0264583 X:0.19341 Y:0.00926042 X:0.198173 Y:0.022225 X:0.205052 Y:0.0137583 X:0.202142 Y:0.0214312 X:0.195792 Y:0.015875 X:0.192881 Y:0.0269875 X:0.190235 Y:0.00687917 X:0.19976 Y:0.0195792 X:0.204258 Y:0.00873125 X:0.209285 Y:0.0269875 X:0.20664 Y:0.0116417 X:0.20664 Y:0.0264583 X:0.20955 Y:0.00635 X:0.201083 Y:0.0179917 X:0.195527 Y:0.00873125 2 | P2P X:0.165365 Y:0.00608542 X:0.157692 Y:0.0243417 X:0.170127 Y:0.0216958 X:0.170392 Y:0.0127 3 | P2P X:0.127 Y:0.00608542 X:0.132556 Y:0.0150812 X:0.126735 Y:0.0259292 X:0.134673 Y:0.01905 X:0.140758 Y:0.0269875 X:0.137319 Y:0.0169333 X:0.142346 Y:0.00740833 X:0.134937 Y:0.0137583 X:0.129646 Y:0.00635 4 | P2P X:0.0865187 Y:0.00608542 X:0.0878417 Y:0.0145521 X:0.0907521 Y:0.0259292 X:0.0981604 Y:0.0137583 X:0.103187 Y:0.0254 X:0.110067 Y:0.0166687 X:0.112448 Y:0.00714375 X:0.105569 Y:0.022225 X:0.101335 Y:0.0079375 X:0.0936625 Y:0.0216958 X:0.0891646 Y:0.00740833 5 | P2P X:0.0584729 Y:0.00608542 X:0.0674687 Y:0.00846667 X:0.0590021 Y:0.0145521 X:0.0674687 Y:0.0169333 X:0.0582083 Y:0.0246062 X:0.0709083 Y:0.0269875 X:0.0709083 Y:0.00635 6 | P2P X:0.0391583 Y:0.00608542 X:0.0381 Y:0.0246062 X:0.0288396 Y:0.0246062 X:0.041275 Y:0.0269875 X:0.041275 Y:0.00635 7 | P2P X:0.0116417 Y:0.00608542 X:0.0105833 Y:0.0246062 X:0.00132292 Y:0.0246062 X:0.0137583 Y:0.0269875 X:0.0137583 Y:0.00635 8 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/drawing_files/s.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0145521 Y:0.01905 X:0.00661458 Y:0.0238125 X:0.00343958 Y:0.0312208 X:0.00978958 Y:0.0367771 X:0.0174625 Y:0.0336021 X:0.00899583 Y:0.0343958 X:0.00714375 Y:0.0261937 X:0.0145521 Y:0.0224896 X:0.0156104 Y:0.0142875 2 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/findContours: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_arm_demos/draw_demo/opencv/findContours -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/house2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_arm_demos/draw_demo/opencv/house2.png -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/house3.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0531812 Y:0.0473604 X:0.0518583 Y:0.060325 X:0.0521229 Y:0.0724958 X:0.0526521 Y:0.060325 X:0.0529167 Y:0.0484187 X:0.0613833 Y:0.047625 X:0.0682625 Y:0.0722312 X:0.0685271 Y:0.0470958 2 | P2P X:0.0518583 Y:0.0145521 X:0.0510646 Y:0.0256646 X:0.0674687 Y:0.0288396 X:0.0679979 Y:0.0148167 X:0.0560917 Y:0.0142875 3 | P2P X:0.0566208 Y:0.00502708 X:0.0497417 Y:0.00978958 X:0.0383646 Y:0.0182562 X:0.0309562 Y:0.0235479 X:0.0301625 Y:0.0717021 X:0.0309562 Y:0.0246062 X:0.041275 Y:0.0169333 X:0.0515937 Y:0.00978958 X:0.0619125 Y:0.00899583 X:0.0719667 Y:0.0153458 X:0.0844021 Y:0.0235479 X:0.0886354 Y:0.0727604 X:0.0891646 Y:0.0259292 X:0.0775229 Y:0.0179917 X:0.0621771 Y:0.0079375 4 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/house3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_arm_demos/draw_demo/opencv/house3.png -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/longhorn.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0896937 Y:0.0288396 X:0.0820208 Y:0.0328083 X:0.0732896 Y:0.0394229 X:0.0711729 Y:0.0473604 X:0.0611187 Y:0.0473604 X:0.0566208 Y:0.0576792 X:0.0566208 Y:0.0672042 X:0.0518583 Y:0.0751417 X:0.0433917 Y:0.0738187 X:0.0420687 Y:0.0650875 X:0.0407458 Y:0.05715 X:0.0370417 Y:0.0492125 X:0.0283104 Y:0.0486833 X:0.0259292 Y:0.0404812 X:0.0177271 Y:0.0338667 X:0.0103187 Y:0.0296333 X:0.0015875 Y:0.028575 X:0.0103187 Y:0.0296333 X:0.0174625 Y:0.0338667 X:0.0248708 Y:0.0399521 X:0.0264583 Y:0.0478896 X:0.0346604 Y:0.0484187 X:0.0407458 Y:0.0574146 X:0.0415396 Y:0.0661458 X:0.0441854 Y:0.0746125 X:0.0526521 Y:0.0748771 X:0.0566208 Y:0.0672042 X:0.0563562 Y:0.0584729 X:0.060325 Y:0.0497417 X:0.0685271 Y:0.0486833 X:0.0722312 Y:0.0402167 X:0.079375 Y:0.034925 X:0.0862542 Y:0.0304271 X:0.0976312 Y:0.0280458 2 | P2P X:0.0902229 Y:0.0254 X:0.0820208 Y:0.0280458 X:0.0743479 Y:0.0322792 X:0.066675 Y:0.0367771 X:0.0584729 Y:0.0370417 X:0.0505354 Y:0.0357187 X:0.0407458 Y:0.0359833 X:0.03175 Y:0.0373062 X:0.0238125 Y:0.0333375 X:0.0134937 Y:0.0269875 X:0.00343958 Y:0.0254 X:0.0127 Y:0.0291042 X:0.0232833 Y:0.0370417 X:0.0304271 Y:0.041275 X:0.0373062 Y:0.0455083 X:0.0415396 Y:0.0560917 X:0.0436562 Y:0.0642937 X:0.0439208 Y:0.0724958 X:0.0521229 Y:0.0735542 X:0.0542396 Y:0.0650875 X:0.0555625 Y:0.0568854 X:0.0590021 Y:0.0489479 X:0.0677333 Y:0.0473604 X:0.0706437 Y:0.0394229 X:0.0780521 Y:0.0343958 X:0.0849312 Y:0.0293687 X:0.0973667 Y:0.0264583 3 | P2P X:0.0886354 Y:0.0243417 X:0.0809625 Y:0.0269875 X:0.0690562 Y:0.0341312 X:0.0600604 Y:0.0359833 X:0.0510646 Y:0.0346604 X:0.0415396 Y:0.0343958 X:0.0333375 Y:0.0362479 X:0.0251354 Y:0.0325437 X:0.0177271 Y:0.0275167 X:0.00873125 Y:0.0243417 X:0.000264583 Y:0.0248708 X:0.00873125 Y:0.0243417 X:0.0174625 Y:0.0275167 X:0.0248708 Y:0.0325437 X:0.0333375 Y:0.0362479 X:0.0415396 Y:0.0343958 X:0.0510646 Y:0.0346604 X:0.0590021 Y:0.0357187 X:0.0672042 Y:0.0351896 X:0.0777875 Y:0.0288396 X:0.0865187 Y:0.0248708 X:0.0957792 Y:0.0238125 4 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/longhorn2.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0105833 Y:0.0187854 X:0.0193146 Y:0.0267229 X:0.0216958 Y:0.034925 X:0.0301625 Y:0.0351896 X:0.0336021 Y:0.0457729 X:0.0336021 Y:0.0545042 X:0.0367771 Y:0.0619125 X:0.0439208 Y:0.0579437 X:0.0436562 Y:0.0489479 X:0.0465667 Y:0.0410104 X:0.053975 Y:0.0365125 X:0.05715 Y:0.0280458 X:0.0650875 Y:0.0206375 X:0.0727604 Y:0.0166687 X:0.0642937 Y:0.0164042 X:0.0568854 Y:0.0219604 X:0.0497417 Y:0.0256646 X:0.0418042 Y:0.0243417 X:0.0330729 Y:0.0240771 X:0.0248708 Y:0.0246062 X:0.0177271 Y:0.01905 X:0.00873125 Y:0.0142875 2 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/longhorn2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_arm_demos/draw_demo/opencv/longhorn2.jpg -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/longhornRE.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.0896937 Y:0.0288396 X:0.0820208 Y:0.0328083 X:0.0732896 Y:0.0394229 X:0.0711729 Y:0.0473604 X:0.0611187 Y:0.0473604 X:0.0566208 Y:0.0576792 X:0.0566208 Y:0.0672042 X:0.0518583 Y:0.0751417 X:0.0433917 Y:0.0738187 X:0.0420687 Y:0.0650875 X:0.0407458 Y:0.05715 X:0.0370417 Y:0.0492125 X:0.0283104 Y:0.0486833 X:0.0259292 Y:0.0404812 X:0.0177271 Y:0.0338667 X:0.0103187 Y:0.0296333 X:0.0015875 Y:0.028575 X:0.0103187 Y:0.0296333 X:0.0174625 Y:0.0338667 X:0.0248708 Y:0.0399521 X:0.0264583 Y:0.0478896 X:0.0346604 Y:0.0484187 X:0.0407458 Y:0.0574146 X:0.0415396 Y:0.0661458 X:0.0441854 Y:0.0746125 X:0.0526521 Y:0.0748771 X:0.0566208 Y:0.0672042 X:0.0563562 Y:0.0584729 X:0.060325 Y:0.0497417 X:0.0685271 Y:0.0486833 X:0.0722312 Y:0.0402167 X:0.079375 Y:0.034925 X:0.0862542 Y:0.0304271 X:0.0976312 Y:0.0280458 2 | P2P X:0.0902229 Y:0.0254 X:0.0820208 Y:0.0280458 X:0.0743479 Y:0.0322792 X:0.066675 Y:0.0367771 X:0.0584729 Y:0.0370417 X:0.0505354 Y:0.0357187 X:0.0407458 Y:0.0359833 X:0.03175 Y:0.0373062 X:0.0238125 Y:0.0333375 X:0.0134937 Y:0.0269875 X:0.00343958 Y:0.0254 X:0.0127 Y:0.0291042 X:0.0232833 Y:0.0370417 X:0.0304271 Y:0.041275 X:0.0373062 Y:0.0455083 X:0.0415396 Y:0.0560917 X:0.0436562 Y:0.0642937 X:0.0439208 Y:0.0724958 X:0.0521229 Y:0.0735542 X:0.0542396 Y:0.0650875 X:0.0555625 Y:0.0568854 X:0.0590021 Y:0.0489479 X:0.0677333 Y:0.0473604 X:0.0706437 Y:0.0394229 X:0.0780521 Y:0.0343958 X:0.0849312 Y:0.0293687 X:0.0973667 Y:0.0264583 3 | P2P X:0.0886354 Y:0.0243417 X:0.0809625 Y:0.0269875 X:0.0690562 Y:0.0341312 X:0.0600604 Y:0.0359833 X:0.0510646 Y:0.0346604 X:0.0415396 Y:0.0343958 X:0.0333375 Y:0.0362479 X:0.0251354 Y:0.0325437 X:0.0177271 Y:0.0275167 X:0.00873125 Y:0.0243417 X:0.000264583 Y:0.0248708 X:0.00873125 Y:0.0243417 X:0.0174625 Y:0.0275167 X:0.0248708 Y:0.0325437 X:0.0333375 Y:0.0362479 X:0.0415396 Y:0.0343958 X:0.0510646 Y:0.0346604 X:0.0590021 Y:0.0357187 X:0.0672042 Y:0.0351896 X:0.0777875 Y:0.0288396 X:0.0865187 Y:0.0248708 X:0.0957792 Y:0.0238125 4 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/maxwell.cdcode: -------------------------------------------------------------------------------- 1 | P2P X:0.190235 Y:0.0264583 X:0.19341 Y:0.00926042 X:0.198173 Y:0.022225 X:0.205052 Y:0.0137583 X:0.202142 Y:0.0214312 X:0.195792 Y:0.015875 X:0.192881 Y:0.0269875 X:0.190235 Y:0.00687917 X:0.19976 Y:0.0195792 X:0.204258 Y:0.00873125 X:0.209285 Y:0.0269875 X:0.20664 Y:0.0116417 X:0.20664 Y:0.0264583 X:0.20955 Y:0.00635 X:0.201083 Y:0.0179917 X:0.195527 Y:0.00873125 2 | P2P X:0.165365 Y:0.00608542 X:0.157692 Y:0.0243417 X:0.170127 Y:0.0216958 X:0.170392 Y:0.0127 3 | P2P X:0.127 Y:0.00608542 X:0.132556 Y:0.0150812 X:0.126735 Y:0.0259292 X:0.134673 Y:0.01905 X:0.140758 Y:0.0269875 X:0.137319 Y:0.0169333 X:0.142346 Y:0.00740833 X:0.134937 Y:0.0137583 X:0.129646 Y:0.00635 4 | P2P X:0.0865187 Y:0.00608542 X:0.0878417 Y:0.0145521 X:0.0907521 Y:0.0259292 X:0.0981604 Y:0.0137583 X:0.103187 Y:0.0254 X:0.110067 Y:0.0166687 X:0.112448 Y:0.00714375 X:0.105569 Y:0.022225 X:0.101335 Y:0.0079375 X:0.0936625 Y:0.0216958 X:0.0891646 Y:0.00740833 5 | P2P X:0.0584729 Y:0.00608542 X:0.0674687 Y:0.00846667 X:0.0590021 Y:0.0145521 X:0.0674687 Y:0.0169333 X:0.0582083 Y:0.0246062 X:0.0709083 Y:0.0269875 X:0.0709083 Y:0.00635 6 | P2P X:0.0391583 Y:0.00608542 X:0.0381 Y:0.0246062 X:0.0288396 Y:0.0246062 X:0.041275 Y:0.0269875 X:0.041275 Y:0.00635 7 | P2P X:0.0116417 Y:0.00608542 X:0.0105833 Y:0.0246062 X:0.00132292 Y:0.0246062 X:0.0137583 Y:0.0269875 X:0.0137583 Y:0.00635 8 | -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/maxwell.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_arm_demos/draw_demo/opencv/maxwell.png -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/mona.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_arm_demos/draw_demo/opencv/mona.jpg -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/transforms: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_arm_demos/draw_demo/opencv/transforms -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/wedding_resized.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_arm_demos/draw_demo/opencv/wedding_resized.jpg -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/opencv/wow.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_arm_demos/draw_demo/opencv/wow.png -------------------------------------------------------------------------------- /segbot_arm_demos/draw_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | draw_demo 4 | 0.1.4 5 | The BWI Drawing Arm project 6 | Maxwell Svetlik 7 | BSD 8 | 9 | catkin 10 | bwi_moveit_utils 11 | bwi_perception 12 | kinova_msgs 13 | roscpp 14 | rospy 15 | std_msgs 16 | 17 | roscpp 18 | rospy 19 | std_msgs 20 | kinova_msgs 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /segbot_arm_demos/mimic_motion/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mimic_motion) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | if( $ENV{SEGBOT_HAS_ARM} ) 7 | find_package(catkin REQUIRED COMPONENTS 8 | agile_grasp 9 | bwi_moveit_utils 10 | bwi_perception 11 | geometry_msgs 12 | roscpp 13 | rospy 14 | std_msgs 15 | sensor_msgs 16 | message_generation 17 | kinova_msgs 18 | actionlib 19 | pcl_ros 20 | pcl_conversions 21 | tf_conversions 22 | moveit_ros_planning_interface 23 | moveit_msgs 24 | segbot_arm_manipulation 25 | 26 | ) 27 | 28 | find_package(PCL 1.7 REQUIRED) 29 | 30 | include_directories(${PCL_INCLUDE_DIRS}) 31 | link_directories(${PCL_LIBRARY_DIRS}) 32 | add_definitions(${PCL_DEFINITIONS}) 33 | 34 | catkin_package( 35 | DEPENDS 36 | ) 37 | 38 | include_directories( 39 | ${catkin_INCLUDE_DIRS} 40 | ) 41 | 42 | add_executable(push_button_demo src/push_button_demo.cpp) 43 | target_link_libraries(push_button_demo ${catkin_LIBRARIES} ) 44 | 45 | add_executable(candy_demo src/candy_demo.cpp) 46 | target_link_libraries(candy_demo ${catkin_LIBRARIES} ) 47 | 48 | add_executable(touch_tabletop_object_demo src/touch_tabletop_object_demo.cpp) 49 | target_link_libraries(touch_tabletop_object_demo ${catkin_LIBRARIES} ) 50 | 51 | add_executable(touch_detect_tabletop_demo src/touch_detect_tabletop_demo.cpp) 52 | target_link_libraries(touch_detect_tabletop_demo ${catkin_LIBRARIES} ) 53 | endif() 54 | -------------------------------------------------------------------------------- /segbot_arm_demos/mimic_motion/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mimic_motion 4 | 0.0.0 5 | The mimic_motion package 6 | 7 | Jivko Sinapov 8 | BSD 9 | 10 | 11 | catkin 12 | agile_grasp 13 | roscpp 14 | rospy 15 | std_msgs 16 | kinova_msgs 17 | bwi_moveit_utils 18 | moveit_msgs 19 | bwi_perception 20 | segbot_arm_manipulation 21 | 22 | roscpp 23 | rospy 24 | std_msgs 25 | kinova_msgs 26 | bwi_perception 27 | segbot_arm_manipulation 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /segbot_arm_joystick/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(segbot_arm_joystick) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | if( $ENV{SEGBOT_HAS_ARM} ) 7 | find_package(catkin REQUIRED COMPONENTS 8 | actionlib 9 | actionlib_msgs 10 | bwi_msgs 11 | bwi_services 12 | kinova_msgs 13 | roscpp 14 | rospy 15 | std_msgs 16 | segbot_arm_manipulation 17 | ) 18 | 19 | catkin_package( 20 | CATKIN_DEPENDS actionlib_msgs 21 | bwi_msgs 22 | bwi_services 23 | kinova_msgs 24 | segbot_arm_manipulation 25 | std_msgs 26 | ) 27 | 28 | ########### 29 | ## Build ## 30 | ########### 31 | 32 | include_directories( 33 | ${catkin_INCLUDE_DIRS} 34 | ) 35 | 36 | add_executable(${PROJECT_NAME} src/${PROJECT_NAME}.cpp) 37 | 38 | target_link_libraries(${PROJECT_NAME} 39 | ${catkin_LIBRARIES} 40 | ) 41 | 42 | ############# 43 | ## Install ## 44 | ############# 45 | 46 | install(TARGETS ${PROJECT_NAME} 47 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 48 | ) 49 | endif() 50 | -------------------------------------------------------------------------------- /segbot_arm_joystick/README.md: -------------------------------------------------------------------------------- 1 | # Controller for BWI Kinova arm, Segway base 2 | 3 | Uses the "joy" ROS package. 4 | 5 | ## Arm controls: 6 | Linear velocity x - Left axis stick left/right 7 | Linear velocity y - Left axis stick up/down 8 | Linear velocity z - Left trigger/Right trigger 9 | 10 | Angular velocity x - Right axis stick left/right 11 | Angular velocity y - Right axis stick up/down 12 | Angular velocity z - Left back button/Right back button 13 | 14 | ### Fingers: 15 | Open slowly - 4-direction pad LEFT 16 | Close slowly - 4-direction pad RIGHT 17 | Open fully - 4-direction pad UP 18 | Close fully - 4-direction pad DOWN 19 | 20 | Home arm - Center button 21 | Switch modes - Back + Start buttons 22 | 23 | ## Segway base controls: 24 | Forward/Backward - Left axis stick up/down 25 | Turn - Right axis stick left/right 26 | 27 | Increase/Decrease speed - Y/A 28 | Increase/Decrease turn speed - X/B 29 | -------------------------------------------------------------------------------- /segbot_arm_joystick/launch/segbot_arm_joystick.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /segbot_arm_joystick/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | segbot_arm_joystick 4 | 0.0.1 5 | Xbox controller for BWI Kinova arm, Segway base 6 | 7 | Benjamin Singer 8 | Palak Hirpara 9 | 10 | 11 | BSD 12 | 13 | 14 | catkin 15 | bwi_msgs 16 | bwi_services 17 | kinova_msgs 18 | moveit_msgs 19 | roscpp 20 | rospy 21 | moveit_msgs 22 | segbot_arm_manipulation 23 | std_msgs 24 | actionlib_msgs 25 | actionlib 26 | 27 | actionlib_msgs 28 | bwi_msgs 29 | bwi_services 30 | bwi_moveit_utils 31 | kinova_msgs 32 | message_runtime 33 | roscpp 34 | rospy 35 | std_msgs 36 | segbot_arm_manipulation 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /segbot_arm_manipulation/action/Handover.action: -------------------------------------------------------------------------------- 1 | string GIVE="handover_give" # used for handing over an object based on haptic feedback 2 | string RECEIVE="handover_receive # used when robot listens for haptics feedback and grabs object from human 3 | 4 | string type 5 | float32 timeout_seconds 6 | 7 | 8 | --- 9 | # Result 10 | bool success 11 | string error_msg 12 | --- 13 | # Feedback 14 | bool status 15 | -------------------------------------------------------------------------------- /segbot_arm_manipulation/action/LiftVerify.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | sensor_msgs/PointCloud2 tgt_cloud 3 | int8 bins 4 | --- 5 | #result definition 6 | bool success 7 | --- 8 | #feedback 9 | bool success 10 | -------------------------------------------------------------------------------- /segbot_arm_manipulation/action/ObjReplacement.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | --- 3 | # Result 4 | bool success 5 | string error_msg 6 | --- 7 | # Feedback 8 | bool status 9 | -------------------------------------------------------------------------------- /segbot_arm_manipulation/action/Press.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | sensor_msgs/PointCloud2 tgt_cloud 3 | float32[4] cloud_plane_coef 4 | --- 5 | #result definition 6 | bool success 7 | --- 8 | #feedback 9 | bool object_moved 10 | -------------------------------------------------------------------------------- /segbot_arm_manipulation/action/Push.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | sensor_msgs/PointCloud2 tgt_cloud 3 | sensor_msgs/PointCloud2 cloud_plane 4 | float32[4] cloud_plane_coef 5 | --- 6 | #result definition 7 | bool success 8 | --- 9 | #feedback 10 | bool object_moved 11 | -------------------------------------------------------------------------------- /segbot_arm_manipulation/action/Shake.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | sensor_msgs/PointCloud2 tgt_cloud 3 | sensor_msgs/PointCloud2 cloud_plane 4 | bool verified 5 | --- 6 | #result definition 7 | bool success 8 | --- 9 | #feedback 10 | bool object_moved 11 | -------------------------------------------------------------------------------- /segbot_arm_manipulation/action/TabletopApproach.action: -------------------------------------------------------------------------------- 1 | string CIRCULAR="circular" 2 | string RECTANGULAR="rectangular" 3 | 4 | # Goal 5 | string command 6 | string table_type 7 | --- 8 | # Result 9 | bool success 10 | string error_msg 11 | --- 12 | # Feedback 13 | bool status 14 | -------------------------------------------------------------------------------- /segbot_arm_manipulation/action/TabletopGrasp.action: -------------------------------------------------------------------------------- 1 | # constants for which grasps should be filtered 2 | string SIDEWAY_GRASP_FILTER="side" 3 | string TOPDOWN_GRASP_FILTER="top" 4 | 5 | 6 | # constants for which function should be optimized when selecting a grasp 7 | string RANDOM_SELECTION="random" 8 | string CLOSEST_ORIENTATION_SELECTION="closest_orientation" 9 | string CLOSEST_JOINTSPACE_SELECTION="closest_joint_space" 10 | string CLOSEST_TO_CENTROID_SELECTION="closest_to_centroid" 11 | 12 | # constants for which method to use to generate grasps 13 | string HEURISTIC="heuristic_grasp" 14 | string AGILE_GRASP="agile_grasp" 15 | 16 | # Goal 17 | sensor_msgs/PointCloud2 cloud_plane 18 | float32[4] cloud_plane_coef 19 | sensor_msgs/PointCloud2[] cloud_clusters 20 | int32 target_object_cluster_index 21 | 22 | string grasp_generation_method 23 | string grasp_selection_method 24 | string grasp_filter_method 25 | 26 | 27 | --- 28 | # Result 29 | bool success 30 | string error_msg 31 | sensor_msgs/JointState approach_joint_state 32 | geometry_msgs/PoseStamped approach_pose 33 | sensor_msgs/JointState grasp_joint_state 34 | geometry_msgs/PoseStamped grasp_pose 35 | 36 | --- 37 | # Feedback 38 | bool status 39 | -------------------------------------------------------------------------------- /segbot_arm_manipulation/data/mico_jointspace_position_db.txt: -------------------------------------------------------------------------------- 1 | 9 2 | home 1.50, 1.30, 2.30, 4.50, 5.60, 6.70 3 | side_view 5.82, 3.81, 1.05, 3.72, 3.94,-2.30 4 | side_view_approach 4.84, 3.81, 0.64, 2.63, 4.40,-2.30 5 | off_to_right 11.27, 4.05, 1.23, 2.54, 10.5, 3.43 6 | handover_front -0.49,-1.77, 0.26,-1.25, 2.59, 3.09 7 | safe -1.49,-1.80,-0.13,-2.17, 0.57, 2.68 8 | point_straight 3.25, 3.99, 0.83, 7.37, 4.15, 4.02 9 | point_right 3.84, 3.72, 0.78, 6.33, 4.24, 4.02 10 | point_left 1.75, 3.80, 0.61, 7.16, 4.17, 3.91 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /segbot_arm_manipulation/data/mico_toolspace_position_db.txt: -------------------------------------------------------------------------------- 1 | 3 2 | home 1.5,1.3,2.3,4.5,5.6,6.7,0.0 3 | side_view 0.03,-0.13,0.28,0.707107,0.0,0.707107,0.0 4 | side_view_approach 0.25,-0.20,0.25,0.707107,0.0,0.707107,0.0 5 | handover_front 0.359293490648,0.00463120825589,0.488316357136,0.47277646971,0.50527286096,0.55337595341,0.463634338518 6 | point_straight 0.25,-0.20,0.25,0.707107,0.0,0.707107,0.0 7 | -------------------------------------------------------------------------------- /segbot_arm_manipulation/include/segbot_arm_manipulation/arm_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef ARM_UTILS_H 2 | #define ARM_UTILS_H 3 | 4 | #include 5 | 6 | //actions 7 | #include 8 | #include "kinova_msgs/SetFingersPositionAction.h" 9 | #include "kinova_msgs/ArmPoseAction.h" 10 | #include "kinova_msgs/ArmJointAnglesAction.h" 11 | #include "kinova_msgs/HomeArm.h" 12 | 13 | #include 14 | 15 | #include 16 | 17 | #include "bwi_perception/SetObstacles.h" 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | #include "bwi_manipulation/ArmPositionDB.h" 24 | 25 | 26 | #define NUM_JOINTS 8 27 | 28 | using namespace std; 29 | 30 | 31 | namespace segbot_arm_manipulation { 32 | using namespace pcl; 33 | typedef pcl::PointXYZRGB PointT; 34 | typedef pcl::PointCloud PointCloudT; 35 | 36 | 37 | sensor_msgs::JointState values_to_joint_state(std::vector joint_values); 38 | 39 | std::vector getJointAngleDifferences(sensor_msgs::JointState A, sensor_msgs::JointState B); 40 | 41 | std::vector getJointAngleDifferences(kinova_msgs::JointAngles A, kinova_msgs::JointAngles B); 42 | 43 | double getDistanceDifferences(geometry_msgs::Pose A, Eigen::Vector4f B); 44 | 45 | // put the boxes around the collision objects 46 | std::vector 47 | get_collision_boxes(std::vector obstacles); 48 | 49 | kinova_msgs::JointAngles state_to_angles(const sensor_msgs::JointState &state); 50 | 51 | 52 | double quat_angular_difference(const geometry_msgs::Quaternion &a, const geometry_msgs::Quaternion &b); 53 | } 54 | #endif 55 | -------------------------------------------------------------------------------- /segbot_arm_manipulation/launch/segbot_arm_manipulation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | [-1, 1, -1, 1, -1, 1.5] 29 | [0, 0.445417, 0.895323, 0.215, 30 | 1, 0, 0, -0.015, 31 | 0, 0.895323, -0.445417, 0.23, 32 | 0, 0, 0, 1] 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /segbot_arm_manipulation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | segbot_arm_manipulation 4 | 0.0.1 5 | Segbot arm manipulation 6 | 7 | Jivko Sinapov 8 | 9 | BSD 10 | 11 | catkin 12 | 13 | actionlib 14 | actionlib_msgs 15 | agile_grasp 16 | bwi_manipulation 17 | bwi_moveit_utils 18 | bwi_msgs 19 | bwi_perception 20 | geometry_msgs 21 | kinova_msgs 22 | kinova_driver 23 | moveit_ros_planning_interface 24 | moveit_msgs 25 | move_base_msgs 26 | roscpp 27 | rospy 28 | std_msgs 29 | std_srvs 30 | tf 31 | 32 | 33 | 34 | message_generation 35 | message_runtime 36 | 37 | 38 | -------------------------------------------------------------------------------- /segbot_arm_manipulation/setup.py: -------------------------------------------------------------------------------- 1 | # ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['segbot_arm_manipulation'], 9 | package_dir={'': 'src'} 10 | #requires=['std_msgs', 'rospy', 'jaco_msgs', 'geometry_msgs', 'segbot_arm_manipulation', 'sensor_msgs','bwi_perception'] 11 | ) 12 | 13 | setup(**setup_args) 14 | -------------------------------------------------------------------------------- /segbot_arm_manipulation/src/segbot_arm_manipulation/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_arm_manipulation/src/segbot_arm_manipulation/__init__.py -------------------------------------------------------------------------------- /segbot_arm_manipulation/srv/NavSafety.srv: -------------------------------------------------------------------------------- 1 | bool getSafe 2 | --- 3 | bool safe 4 | -------------------------------------------------------------------------------- /segbot_arm_manipulation/srv/iSpyDetectTouch.srv: -------------------------------------------------------------------------------- 1 | # sensory data (i.e., the response of table top object detection) 2 | sensor_msgs/PointCloud2 cloud_plane 3 | float32[4] cloud_plane_coef 4 | sensor_msgs/PointCloud2[] objects 5 | --- 6 | int32 detected_touch_index 7 | bool success 8 | -------------------------------------------------------------------------------- /segbot_arm_manipulation/srv/iSpyFaceTable.srv: -------------------------------------------------------------------------------- 1 | # which table to face 2 | int32 table_index 3 | --- 4 | bool success 5 | -------------------------------------------------------------------------------- /segbot_arm_manipulation/srv/iSpyReorderClouds.srv: -------------------------------------------------------------------------------- 1 | string axis 2 | bool forward 3 | string frame_id 4 | sensor_msgs/PointCloud2[] clouds 5 | --- 6 | sensor_msgs/PointCloud2[] ordered_clouds -------------------------------------------------------------------------------- /segbot_arm_manipulation/srv/iSpyTouch.srv: -------------------------------------------------------------------------------- 1 | # sensory data (i.e., the response of table top object detection) 2 | sensor_msgs/PointCloud2 cloud_plane 3 | float32[4] cloud_plane_coef 4 | sensor_msgs/PointCloud2[] objects 5 | 6 | # which object to touch (-1 for retract) 7 | int32 touch_index 8 | --- 9 | bool success 10 | -------------------------------------------------------------------------------- /segbot_arm_tasks/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | segbot_arm_tasks 4 | 0.0.0 5 | This package holds independent tasks that can be performed by the arm. Initial examples will be actionlib servers with tasks such as shake, lift, etc. 6 | 7 | 8 | 9 | 10 | Maxwell J Svetlik 11 | 12 | 13 | 14 | 15 | 16 | GPLv3 17 | 18 | 19 | catkin 20 | actionlib_msgs 21 | plan_execution 22 | bwi_moveit_utils 23 | bwi_msgs 24 | bwi_services 25 | 26 | kinova_msgs 27 | roscpp 28 | rospy 29 | segbot_arm_manipulation 30 | bwi_perception 31 | std_msgs 32 | message_generation 33 | move_base_msgs 34 | moveit_msgs 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /segbot_arm_tutorials/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(segbot_arm_tutorials) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | if( $ENV{SEGBOT_HAS_ARM} ) 7 | find_package(catkin REQUIRED COMPONENTS 8 | actionlib_msgs 9 | actionlib 10 | kinova_msgs 11 | moveit_msgs 12 | moveit_ros_planning_interface 13 | bwi_moveit_utils 14 | moveit_msgs 15 | nav_msgs 16 | move_base_msgs 17 | pcl_ros 18 | pcl_conversions 19 | roscpp 20 | rospy 21 | bwi_perception 22 | segbot_arm_manipulation 23 | std_msgs 24 | ) 25 | 26 | catkin_package( 27 | CATKIN_DEPENDS 28 | ) 29 | 30 | include_directories( 31 | ${catkin_INCLUDE_DIRS} 32 | ) 33 | 34 | add_executable(ex1_subscribing_to_topics src/ex1_subscribing_to_topics.cpp) 35 | target_link_libraries(ex1_subscribing_to_topics ${catkin_LIBRARIES} ) 36 | 37 | add_executable(ex2_gripper src/ex2_gripper_control.cpp) 38 | target_link_libraries(ex2_gripper ${catkin_LIBRARIES} ) 39 | 40 | add_executable(ex3_home_arm src/ex3_home_arm.cpp) 41 | target_link_libraries(ex3_home_arm ${catkin_LIBRARIES} ) 42 | 43 | add_executable(ex4_cartesian_vel_control src/ex4_cartesian_vel_control.cpp) 44 | target_link_libraries(ex4_cartesian_vel_control ${catkin_LIBRARIES} ) 45 | 46 | add_executable(ex5_angular_vel_control src/ex5_angular_vel_control.cpp) 47 | target_link_libraries(ex5_angular_vel_control ${catkin_LIBRARIES} ) 48 | 49 | add_executable(ex6_detect_force src/ex6_detect_force.cpp) 50 | target_link_libraries(ex6_detect_force ${catkin_LIBRARIES} ) 51 | 52 | add_executable(ex7_cartesian_pose_control src/ex7_cartesian_pose_control.cpp) 53 | target_link_libraries(ex7_cartesian_pose_control ${catkin_LIBRARIES} ) 54 | 55 | add_executable(ex8_joint_angles_control src/ex8_joint_angles_control.cpp) 56 | target_link_libraries(ex8_joint_angles_control ${catkin_LIBRARIES} ) 57 | 58 | add_executable(ex9_waypoints src/ex9_waypoints.cpp) 59 | target_link_libraries(ex9_waypoints ${catkin_LIBRARIES} ) 60 | endif() 61 | -------------------------------------------------------------------------------- /segbot_arm_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | segbot_arm_tutorials 4 | 0.0.0 5 | This package provides a set of examples for controlling the robot arm. 6 | 7 | 8 | Jivko Sinapov 9 | 10 | 11 | GPLv3 12 | 13 | 14 | catkin 15 | kinova_msgs 16 | moveit_msgs 17 | roscpp 18 | rospy 19 | std_msgs 20 | actionlib_msgs 21 | message_generation 22 | bwi_moveit_utils 23 | bwi_perception 24 | move_base_msgs 25 | segbot_arm_manipulation 26 | bwi_msgs 27 | 28 | kinova_msgs 29 | moveit_msgs 30 | roscpp 31 | rospy 32 | std_msgs 33 | message_runtime 34 | actionlib_msgs 35 | bwi_moveit_utils 36 | bwi_perception 37 | 38 | move_base_msgs 39 | segbot_arm_manipulation 40 | bwi_msgs 41 | 42 | 43 | -------------------------------------------------------------------------------- /segbot_arm_tutorials/src/ex6_detect_force.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | //our own arm library 10 | #include 11 | #include 12 | 13 | 14 | //true if Ctrl-C is pressed 15 | bool g_caught_sigint=false; 16 | 17 | /* what happens when ctr-c is pressed */ 18 | void sig_handler(int sig) { 19 | g_caught_sigint = true; 20 | ROS_INFO("caught sigint, init shutdown sequence..."); 21 | ros::shutdown(); 22 | exit(1); 23 | }; 24 | 25 | 26 | // Blocking call for user input 27 | void pressEnter(std::string message){ 28 | std::cout << message; 29 | while (true){ 30 | char c = std::cin.get(); 31 | if (c == '\n') 32 | break; 33 | else if (c == 'q'){ 34 | ros::shutdown(); 35 | exit(1); 36 | } 37 | else { 38 | std::cout << message; 39 | } 40 | } 41 | } 42 | 43 | 44 | 45 | int main(int argc, char **argv) { 46 | // Intialize ROS with this node name 47 | ros::init(argc, argv, "ex6_detect_force"); 48 | 49 | ros::NodeHandle n; 50 | 51 | segbot_arm_manipulation::Mico mico(n); 52 | 53 | //register ctrl-c 54 | signal(SIGINT, sig_handler); 55 | 56 | //open the hand 57 | mico.open_hand(); 58 | int hand_state = 0; //0 for open, 1 for closed 59 | 60 | double force_threshold = 3.0; 61 | double timeout = 30.0; 62 | 63 | while (ros::ok()){ 64 | ROS_INFO("Waiting for force..."); 65 | bool force_detected = mico.wait_for_force(force_threshold, timeout); 66 | 67 | if (force_detected){ 68 | if (hand_state == 0){ 69 | mico.close_hand(); 70 | hand_state = 1; 71 | } 72 | else { 73 | mico.open_hand(); 74 | hand_state = 0; 75 | } 76 | } 77 | } 78 | 79 | ros::shutdown(); 80 | } 81 | -------------------------------------------------------------------------------- /segbot_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html 2 | cmake_minimum_required(VERSION 2.8.3) 3 | project(segbot_description) 4 | 5 | find_package(catkin REQUIRED) 6 | catkin_package() 7 | 8 | #install all directories 9 | foreach(dir gazebo meshes robots urdf) 10 | install(DIRECTORY ${dir}/ 11 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) 12 | endforeach() 13 | -------------------------------------------------------------------------------- /segbot_description/meshes/hokuyo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_description/meshes/hokuyo.png -------------------------------------------------------------------------------- /segbot_description/meshes/xtion/xtion_mount_base_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_description/meshes/xtion/xtion_mount_base_link.STL -------------------------------------------------------------------------------- /segbot_description/meshes/xtion/xtion_pivot_link.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_description/meshes/xtion/xtion_pivot_link.STL -------------------------------------------------------------------------------- /segbot_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | segbot_description 3 | 0.3.5 4 | 5 | Contains URDF descriptions of all robot components and sensors for the 6 | segbot, as well as all the different sensor configurations for a segbot. 7 | 8 | 9 | BSD 10 | 11 | http://ros.org/wiki/segbot_description 12 | https://github.com/utexas-bwi/segbot.git 13 | https://github.com/utexas-bwi/segbot/issues 14 | 15 | Piyush Khandelwal 16 | Piyush Khandelwal 17 | Jack O'Quin 18 | 19 | catkin 20 | 21 | pr2_description 22 | 23 | 24 | velodyne_description 25 | velodyne_gazebo_plugins 26 | xacro 27 | 28 | 29 | -------------------------------------------------------------------------------- /segbot_description/robots/segbot_v1.hokuyo_only.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /segbot_description/robots/segbot_v1.no_sensors.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /segbot_description/robots/segbot_v1.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /segbot_description/test/hokuyo_mount.test.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /segbot_description/test/kinect_mount.test.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /segbot_description/test/segbot_chassis.test.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /segbot_description/test/velodyne_mount.test.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /segbot_description/test/xacrodisplay.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 9 | 11 | 13 | 14 | -------------------------------------------------------------------------------- /segbot_description/urdf/components/alienware_alpha.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | Gazebo/Grey 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /segbot_description/urdf/components/aluminium_sheet.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | Gazebo/Grey 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /segbot_description/urdf/components/battery_box.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | Gazebo/Black 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /segbot_description/urdf/components/beam_8020.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | Gazebo/Grey 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /segbot_description/urdf/components/standoff.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | Gazebo/Grey 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /segbot_description/urdf/segbot_wheel.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | transmission_interface/SimpleTransmission 38 | 39 | 40 | hardware_interface/EffortJointInterface 41 | 42 | 1.0 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /segbot_description/urdf/sensors/hokuyo_laser.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | true 10 | ${update_rate} 11 | 0 0 0 0 0 0 12 | false 13 | 14 | 15 | 16 | 17 | 640 18 | 1 19 | ${min_angle} 20 | ${max_angle} 21 | 22 | 23 | 24 | 0.02 25 | ${range} 26 | 0.03 27 | 28 | 29 | 30 | 0.0 31 | true 32 | ${update_rate} 33 | ${ros_topic} 34 | ${name}_link 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /segbot_description/urdf/sensors/kinect_camera.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | true 11 | 20.0 12 | 13 | ${57.0*M_PI/180.0} 14 | 15 | R8G8B8 16 | 640 17 | 480 18 | 19 | 20 | 0.01 21 | 5 22 | 23 | 24 | 25 | true 26 | 20.0 27 | ${name} 28 | rgb/image_raw 29 | rgb/camera_info 30 | depth/image_raw 31 | depth/camera_info 32 | depth/points 33 | ${name}_depth_optical_frame 34 | 0.5 35 | 0.00000001 36 | 0.00000001 37 | 0.00000001 38 | 0.00000001 39 | 0.00000001 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /segbot_description/urdf/sensors/xtion_camera.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | true 11 | 20.0 12 | 13 | ${57.0*M_PI/180.0} 14 | 15 | R8G8B8 16 | 640 17 | 480 18 | 19 | 20 | 0.01 21 | 5 22 | 23 | 24 | 25 | true 26 | 20.0 27 | ${name} 28 | rgb/image_raw 29 | rgb/camera_info 30 | depth/image_raw 31 | depth/camera_info 32 | depth/points 33 | ${name}_depth_optical_frame 34 | 0.5 35 | 0.00000001 36 | 0.00000001 37 | 0.00000001 38 | 0.00000001 39 | 0.00000001 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /segbot_description/urdf/v1/common.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /segbot_description/urdf/v2/common.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /segbot_description/urdf/v3/common.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /segbot_firmware/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package segbot_firmware 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2016-08-27) 6 | ------------------ 7 | 8 | 0.3.4 (2016-08-08) 9 | ------------------ 10 | 11 | 0.3.3 (2015-08-05) 12 | ------------------ 13 | 14 | 0.3.2 (2015-03-31) 15 | ------------------ 16 | 17 | 0.3.1 (2015-03-24) 18 | ------------------ 19 | 20 | 0.3.0 (2015-03-14) 21 | ------------------ 22 | * new arduino firmware for segbot v2. 23 | * Contributors: Jack O'Quin 24 | 25 | 0.2.1 (2014-04-24) 26 | ------------------ 27 | 28 | 0.2.0 (2014-04-17) 29 | ------------------ 30 | 31 | * Initial release to Hydro. 32 | * Arduino firmware for sonar and IR sensors. 33 | -------------------------------------------------------------------------------- /segbot_firmware/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(segbot_firmware) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | # TODO: figure out how to automate Arduino cross-compile and loading. 9 | 10 | # include firmware sources in the distribution package 11 | install(DIRECTORY src 12 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 13 | -------------------------------------------------------------------------------- /segbot_firmware/README.md: -------------------------------------------------------------------------------- 1 | Segbot Arduino firmware 2 | ======================= 3 | 4 | This package contains Arduino firmware for the UTexas BWI segbot 5 | sensor array. 6 | 7 | Access to Serial Ports 8 | ---------------------- 9 | 10 | To access the Arduino serial port, you must be authorized for the 11 | "dialout" group. To verify that you are a member of that group, run 12 | this command: 13 | 14 | $ id | grep dialout 15 | 16 | If you are in the group, "dialout" will appear in the printed output. 17 | 18 | If you are not yet authorized, run this command on a personal Ubuntu 19 | system: 20 | 21 | $ sudo usermod -a -G dialout your_user_id 22 | 23 | **But**, *do not* run that command on any of the BWI robots or lab 24 | machines. They maintain a shared user and group database, so ask a 25 | system administrator to update it for you. 26 | 27 | In either case, logout and login after your group membership has been 28 | updated, and verify that you are now in the "dialout" group. 29 | 30 | Build and Install 31 | ----------------- 32 | 33 | The CMakeLists.txt does not yet support make and install from the 34 | command line. To build and install firmware on the Arduino board, 35 | first install the Arduino development tools. On Ubuntu, run these 36 | commands: 37 | 38 | $ sudo apt-get install arduino 39 | $ mkdir ~/sketchbook 40 | $ cd ~/sketchbook 41 | $ ln -s $(rospack find segbot_firmware)/src/libraries . 42 | 43 | Plug in the USB cable, then run the Arduino GUI: 44 | 45 | $ arduino 46 | 47 | Select ``File > Sketchbook > Libraries`` followed by the desired 48 | firmware version. The standard one is named ``segbot_arduino``. 49 | Then, click the ``->`` icon to compile the microcode and load it into 50 | the controller. 51 | 52 | ROS Driver 53 | ---------- 54 | 55 | When the firmware is loaded, you can run the ROS device driver: 56 | 57 | $ roslaunch segbot_sensors arduino.launch --screen 58 | 59 | The ``segbot_sensors`` package documentation describes the topics 60 | published. 61 | -------------------------------------------------------------------------------- /segbot_firmware/package.xml: -------------------------------------------------------------------------------- 1 | 2 | segbot_firmware 3 | 0.3.5 4 | 5 | Arduino firmware for BWI segbot sensor array. 6 | 7 | 8 | BSD 9 | GPLv3 10 | 11 | http://ros.org/wiki/segbot_firmware 12 | https://github.com/utexas-bwi/segbot.git 13 | https://github.com/utexas-bwi/segbot/issues 14 | 15 | Jack O'Quin 16 | Piyush Khandelwal 17 | Jose Bigio 18 | Jack O'Quin 19 | Tim Eckel (NewPing library) 20 | 21 | catkin 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /segbot_firmware/src/libraries/MPU9150Lib/MPUVector3.cpp: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of MPU9150Lib 4 | // 5 | // Copyright (c) 2013 Pansenti, LLC 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #include "MPUVector3.h" 25 | 26 | void MPUVector3DotProduct(MPUVector3 a, MPUVector3 b, float *d) 27 | { 28 | *d = a[VEC3_X] * b[VEC3_X] + a[VEC3_Y] * b[VEC3_Y] + a[VEC3_Z] * b[VEC3_Z]; 29 | } 30 | 31 | void MPUVector3CrossProduct(MPUVector3 a, MPUVector3 b, MPUVector3 d) 32 | { 33 | d[VEC3_X] = a[VEC3_Y] * b[VEC3_Z] - a[VEC3_Z] * b[VEC3_Y]; 34 | d[VEC3_Y] = a[VEC3_Z] * b[VEC3_X] - a[VEC3_X] * b[VEC3_Z]; 35 | d[VEC3_Z] = a[VEC3_X] * b[VEC3_Y] - a[VEC3_Y] * b[VEC3_X]; 36 | } 37 | -------------------------------------------------------------------------------- /segbot_firmware/src/libraries/MPU9150Lib/MPUVector3.h: -------------------------------------------------------------------------------- 1 | //////////////////////////////////////////////////////////////////////////// 2 | // 3 | // This file is part of MPU9150Lib 4 | // 5 | // Copyright (c) 2013 Pansenti, LLC 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy of 8 | // this software and associated documentation files (the "Software"), to deal in 9 | // the Software without restriction, including without limitation the rights to use, 10 | // copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 11 | // Software, and to permit persons to whom the Software is furnished to do so, 12 | // subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in all 15 | // copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 18 | // INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 19 | // PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 20 | // HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 | // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 22 | // SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 | 24 | #ifndef MPUVECTOR3_H_ 25 | #define MPUVECTOR3_H_ 26 | 27 | #include 28 | #include 29 | 30 | 31 | #define DEGREE_TO_RAD (float)M_PI / 180.0f) 32 | #define RAD_TO_DEGREE (180.0f / (float)M_PI) 33 | 34 | #define VEC3_X 0 // x offset 35 | #define VEC3_Y 1 // y offset 36 | #define VEC3_Z 2 // z offset 37 | 38 | typedef float MPUVector3[3]; 39 | 40 | void MPUVector3DotProduct(MPUVector3 a, MPUVector3 b, float *d); 41 | void MPUVector3CrossProduct(MPUVector3 a, MPUVector3 b, MPUVector3 d); 42 | 43 | 44 | #endif /* MPUVECTOR3_H_ */ 45 | -------------------------------------------------------------------------------- /segbot_firmware/src/libraries/NewPing/NewPing.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_firmware/src/libraries/NewPing/NewPing.h -------------------------------------------------------------------------------- /segbot_firmware/src/libraries/NewPing/keywords.txt: -------------------------------------------------------------------------------- 1 | ################################### 2 | # Syntax Coloring Map For NewPing 3 | ################################### 4 | 5 | ################################### 6 | # Datatypes (KEYWORD1) 7 | ################################### 8 | 9 | NewPing KEYWORD1 10 | 11 | ################################### 12 | # Methods and Functions (KEYWORD2) 13 | ################################### 14 | 15 | ping KEYWORD2 16 | ping_in KEYWORD2 17 | ping_cm KEYWORD2 18 | ping_median KEYWORD2 19 | ping_timer KEYWORD2 20 | check_timer KEYWORD2 21 | timer_us KEYWORD2 22 | timer_ms KEYWORD2 23 | timer_stop KEYWORD2 24 | convert_in KEYWORD2 25 | convert_cm KEYWORD2 26 | 27 | ################################### 28 | # Constants (LITERAL1) 29 | ################################### 30 | -------------------------------------------------------------------------------- /segbot_firmware/src/libraries/segbot1/README.md: -------------------------------------------------------------------------------- 1 | UTexas BWI Segbot Arduino Firmware (1.0.0) 2 | ========================================== 3 | 4 | This directory contains version 1 of the Arduino firmware used for the 5 | UTexas BWI segbot sensor array. 6 | -------------------------------------------------------------------------------- /segbot_firmware/src/libraries/segbot_arduino/README.md: -------------------------------------------------------------------------------- 1 | Standard Arduino Firmware 2 | ========================= 3 | 4 | This directory links to the standard Arduino firmware used for the 5 | UTexas BWI segbot sensor array. 6 | -------------------------------------------------------------------------------- /segbot_firmware/src/libraries/segbot_arduino/segbot_arduino.ino: -------------------------------------------------------------------------------- 1 | ../multidevice/multidevice.ino -------------------------------------------------------------------------------- /segbot_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html 2 | cmake_minimum_required(VERSION 2.8.3) 3 | project(segbot_gazebo) 4 | find_package(catkin REQUIRED) 5 | catkin_package() 6 | 7 | # Install all supporting directories 8 | foreach(dir config launch maps Media worlds) 9 | install(DIRECTORY ${dir}/ 10 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) 11 | endforeach() 12 | 13 | # unit tests are enabled selectively 14 | if (CATKIN_ENABLE_TESTING) 15 | find_package(roslaunch REQUIRED) 16 | roslaunch_add_file_check(launch) 17 | endif () 18 | -------------------------------------------------------------------------------- /segbot_gazebo/Media/models/segbot_test_world/Blinds_Vertical_Pattern_.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_gazebo/Media/models/segbot_test_world/Blinds_Vertical_Pattern_.jpg -------------------------------------------------------------------------------- /segbot_gazebo/Media/models/segbot_test_world/Brick_Antique_.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_gazebo/Media/models/segbot_test_world/Brick_Antique_.jpg -------------------------------------------------------------------------------- /segbot_gazebo/Media/models/segbot_test_world/Wood_Bamboo_Medium_.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_gazebo/Media/models/segbot_test_world/Wood_Bamboo_Medium_.jpg -------------------------------------------------------------------------------- /segbot_gazebo/launch/multiple_segbots.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /segbot_gazebo/launch/segbot_navigation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /segbot_gazebo/launch/segbot_test_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /segbot_gazebo/launch/segbot_test_world_map_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /segbot_gazebo/maps/segbot_test_world.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_gazebo/maps/segbot_test_world.pgm -------------------------------------------------------------------------------- /segbot_gazebo/maps/segbot_test_world.yaml: -------------------------------------------------------------------------------- 1 | image: segbot_test_world.pgm 2 | resolution: 0.025000 3 | origin: [-7.0, -7.0, 0.0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /segbot_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | segbot_gazebo 3 | 0.3.5 4 | bwi_gazebo 5 | 6 | BSD 7 | 8 | http://ros.org/wiki/bwi_gazebo 9 | https://github.com/utexas-bwi/segbot_simulator.git 10 | https://github.com/utexas-bwi/segbot_simulator/issues 11 | 12 | Piyush Khandelwal 13 | Piyush Khandelwal 14 | Jack O'Quin 15 | 16 | catkin 17 | 18 | fake_localization 19 | gazebo_plugins 20 | gazebo_ros 21 | rviz 22 | joint_trajectory_controller 23 | map_server 24 | segbot_bringup 25 | segbot_description 26 | segbot_navigation 27 | 28 | 29 | roslaunch 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /segbot_led/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package segbot_led 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2016-08-27) 6 | ------------------ 7 | * Added need assistance animation and made circular 4-set animation default turn animation 8 | * Added additonal circular variations to turn signals 9 | * Updated led segments and blocked animation. (`#73 10 | `_) 11 | * Added install target for launch directory. 12 | * Contributors: FernandezR, Rolando Fernandez 13 | 14 | 0.3.4 (2016-08-08) 15 | ------------------ 16 | * Added variations for turn signals and adjusted led segments 17 | * Changed blocked animation to use a pulsing red led animation 18 | * Renamed segbot_led launch for the version 3 build 19 | * Moved messages over to bwi_msgs, made modifications to error 20 | handling logic, and condensed service advertisers. 21 | * Fixed compile time message generation error 22 | * Migrating LED control package from bwi_common to segbot. 23 | * New segbot_led package 24 | * Contributors: FernandezR, Jack O'Quin 25 | -------------------------------------------------------------------------------- /segbot_led/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(segbot_led) 3 | find_package(catkin REQUIRED COMPONENTS 4 | actionlib 5 | actionlib_msgs 6 | bwi_msgs 7 | roscpp 8 | serial 9 | std_msgs 10 | ) 11 | 12 | add_definitions(-std=c++11) 13 | 14 | catkin_package( 15 | CATKIN_DEPENDS actionlib_msgs bwi_msgs roscpp serial std_msgs 16 | ) 17 | 18 | include_directories( 19 | ${catkin_INCLUDE_DIRS} ledcomm 20 | ) 21 | 22 | add_executable(led_control_server src/led_control_server.cpp) 23 | add_dependencies(led_control_server 24 | ${catkin_EXPORTED_TARGETS} 25 | ${${PROJECT_NAME}_EXPORTED_TARGETS}) 26 | target_link_libraries(led_control_server ${catkin_LIBRARIES}) 27 | 28 | add_executable(test_led_control_server test/test.cpp) 29 | add_dependencies(test_led_control_server 30 | ${catkin_EXPORTED_TARGETS} 31 | ${${PROJECT_NAME}_EXPORTED_TARGETS}) 32 | target_link_libraries(test_led_control_server ${catkin_LIBRARIES}) 33 | 34 | install(TARGETS 35 | led_control_server 36 | test_led_control_server 37 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 38 | 39 | install(DIRECTORY 40 | launch/ 41 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) 42 | -------------------------------------------------------------------------------- /segbot_led/README.md: -------------------------------------------------------------------------------- 1 | ## Segbot LED Package 2 | 3 | This ROS package contains code for controlling Pololu WS2812B-Based LED Strips. 4 | 5 | ### Configuring Metro Mini Microcontroller 6 | 7 | * Install recent Arduino IDE if not already installed on machine. 8 | 9 | * Add pololu-led-strip-arduino library to the Arduino IDE. 10 | 11 | * Set Board Manager to Arduino UNO 12 | 13 | * Open led_serial_driver.ino and upload to device. 14 | 15 | ### Configuring udev Rules 16 | 17 | Retrieve the attributes of the microcontroller to create the proper udev rule for the specific device. 18 | 19 | * You will need to monitor udev events to determine the serial port name for the device. Execute the command: 20 | 21 | ``` 22 | udevadm monitor 23 | ``` 24 | 25 | * Plug in the device and annotate what /dev/ name it was assigned. 26 | 27 | * Execute the command below and replace the question marks with the name given to your device. 28 | 29 | ``` 30 | udevadm info --attribute-walk --name=/dev/????? 31 | ``` 32 | 33 | * Retrive the follwing attribute values and replace the values in 99-metromini.rules. 34 | 35 | * ATTRS{idVendor} 36 | * ATTRS{idProduct} 37 | * ATTRS{serial} 38 | 39 | ### Setting Up Segbot LED Configuration 40 | 41 | To utilize the Segbot LED package you must configure the led segments of the robot in the launch file. 42 | The package led animations are configured for use on the Segbots part of the BWI Project, but could 43 | be modified for other robot designs. 44 | 45 | LED Strips must be set up in a similar style as shown below. 46 | 47 | Front 48 | Back 49 | Left 50 | Right 51 | 52 | -------------------------------------------------------------------------------- /segbot_led/arduino/README.md: -------------------------------------------------------------------------------- 1 | ### Configuring Metro Mini Microcontroller 2 | 3 | * Install recent Arduino IDE if not already installed on machine. 4 | 5 | * Add pololu-led-strip-arduino library to the Arduino IDE. 6 | 7 | * Set Board Manager to Arduino UNO 8 | 9 | * Open led_serial_driver.ino and upload to device. -------------------------------------------------------------------------------- /segbot_led/arduino/led_serial_driver/led_serial_driver.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // Sets control pin for led strip 4 | PololuLedStrip<12> ledStrip; 5 | 6 | typedef struct rgb_color_data{ 7 | unsigned char r, g, b; 8 | uint8_t index; 9 | } rgb_color_data; 10 | 11 | union stream_data { 12 | rgb_color rgb; 13 | rgb_color_data rgbd; 14 | }; 15 | 16 | // Default led count 17 | int led_count = 60; 18 | 19 | // Max supported led count is 180 20 | rgb_color colors[180]; 21 | 22 | void setup() { 23 | Serial.begin(115200); 24 | Serial.println("Ready to receive colors."); 25 | Serial.println("Default LED Count"); 26 | Serial.println(led_count); 27 | } 28 | 29 | void improvedSerial() { 30 | uint8_t header = Serial.read(); 31 | 32 | // Flushes colors to led strip 33 | if (header == 'f') { 34 | ledStrip.write(colors, led_count); 35 | 36 | // Sets led colors on strip 37 | } else if (header == 's') { 38 | union stream_data data; 39 | Serial.readBytes((char*)&data, 4); 40 | colors[data.rgbd.index] = data.rgb; 41 | 42 | // Clears Led Strip 43 | } else if (header == 'c') { 44 | for (int i = 0; i < led_count; i++) { 45 | colors[i] = (rgb_color){0,0,0}; 46 | } 47 | ledStrip.write(colors, led_count); 48 | 49 | // Sets led_count value, 0-255 50 | } else if (header == 'l') { 51 | uint8_t count; 52 | Serial.readBytes((uint8_t*)&count, 1); 53 | led_count = count; 54 | } 55 | } 56 | 57 | void loop() { 58 | if (Serial.available()){ 59 | improvedSerial(); 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /segbot_led/arduino/pololu-led-strip-arduino/LICENSE.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2012 Pololu Corporation. For more information, see 2 | 3 | http://www.pololu.com/ 4 | http://forum.pololu.com/ 5 | 6 | Permission is hereby granted, free of charge, to any person 7 | obtaining a copy of this software and associated documentation 8 | files (the "Software"), to deal in the Software without 9 | restriction, including without limitation the rights to use, 10 | copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | copies of the Software, and to permit persons to whom the 12 | Software is furnished to do so, subject to the following 13 | conditions: 14 | 15 | The above copyright notice and this permission notice shall be 16 | included in all copies or substantial portions of the Software. 17 | 18 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 19 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES 20 | OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 21 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 22 | HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 23 | WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 24 | FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR 25 | OTHER DEALINGS IN THE SOFTWARE. -------------------------------------------------------------------------------- /segbot_led/arduino/pololu-led-strip-arduino/PololuLedStrip/PololuLedStrip.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | bool PololuLedStripBase::interruptFriendly = false; -------------------------------------------------------------------------------- /segbot_led/arduino/pololu-led-strip-arduino/PololuLedStrip/examples/LedStripGradient/LedStripGradient.ino: -------------------------------------------------------------------------------- 1 | /* LedStripGradient: Example Arduino sketch that shows 2 | * how to control an Addressable RGB LED Strip from Pololu. 3 | * 4 | * To use this, you will need to plug an Addressable RGB LED 5 | * strip from Pololu into pin 12. After uploading the sketch, 6 | * you should see a pattern on the LED strip that fades from 7 | * green to pink and also moves along the strip. 8 | */ 9 | 10 | #include 11 | 12 | // Create an ledStrip object and specify the pin it will use. 13 | PololuLedStrip<12> ledStrip; 14 | 15 | // Create a buffer for holding the colors (3 bytes per color). 16 | #define LED_COUNT 60 17 | rgb_color colors[LED_COUNT]; 18 | 19 | void setup() 20 | { 21 | } 22 | 23 | void loop() 24 | { 25 | // Update the colors. 26 | byte time = millis() >> 2; 27 | for(uint16_t i = 0; i < LED_COUNT; i++) 28 | { 29 | byte x = time - 8*i; 30 | colors[i] = (rgb_color){ x, 255 - x, x }; 31 | } 32 | 33 | // Write the colors to the LED strip. 34 | ledStrip.write(colors, LED_COUNT); 35 | 36 | delay(10); 37 | } 38 | -------------------------------------------------------------------------------- /segbot_led/images/back.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_led/images/back.JPG -------------------------------------------------------------------------------- /segbot_led/images/front.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_led/images/front.JPG -------------------------------------------------------------------------------- /segbot_led/images/left.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_led/images/left.JPG -------------------------------------------------------------------------------- /segbot_led/images/right.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_led/images/right.JPG -------------------------------------------------------------------------------- /segbot_led/ledcomm/ledcom.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "ros/ros.h" 7 | #include "serial/serial.h" 8 | #include "rgbhsv.h" 9 | 10 | class LedCOM{ 11 | private: 12 | static const uint8_t CLEAR_MSG[1]; 13 | static const uint8_t FLUSH_MSG[1]; 14 | serial::Serial* serial_conn; 15 | void write(const uint8_t[], uint32_t); 16 | 17 | public: 18 | void connect(const std::string, const unsigned long); 19 | void setRGB(const uint8_t, const uint8_t, const uint8_t, const uint8_t); 20 | void setHSV(const uint8_t, float, float, float); 21 | void clear(); 22 | void flush(); 23 | void setLEDCount(const uint8_t); 24 | }; 25 | 26 | const uint8_t LedCOM::CLEAR_MSG[] = {'c'}; 27 | const uint8_t LedCOM::FLUSH_MSG[] = {'f'}; 28 | 29 | void LedCOM::write(const uint8_t data[], const uint32_t data_size) { 30 | serial_conn->write(data, data_size); 31 | serial_conn->flushOutput(); 32 | } 33 | 34 | void LedCOM::connect(const std::string port, const unsigned long baud) { 35 | serial_conn = new serial::Serial(port, baud, serial::Timeout::simpleTimeout(1000)); 36 | } 37 | 38 | void LedCOM::setRGB(const uint8_t index, const uint8_t r, const uint8_t g, const uint8_t b){ 39 | uint8_t data[] = {'s',r,g,b,index}; 40 | write(data, 5); 41 | } 42 | 43 | void LedCOM::setHSV(const uint8_t index, float h, float s, float v) { 44 | float rf, gf, bf; 45 | 46 | HSVtoRGB(rf,gf,bf,h,s,v); 47 | 48 | uint8_t r = 255 * rf; 49 | uint8_t g = 255 * gf; 50 | uint8_t b = 255 * bf; 51 | 52 | uint8_t data[] = {'s',r,g,b,index}; 53 | write(data, 5); 54 | } 55 | 56 | void LedCOM::clear() { write(CLEAR_MSG, 1); } 57 | 58 | void LedCOM::flush() { write(FLUSH_MSG, 1); } 59 | 60 | void LedCOM::setLEDCount(const uint8_t led_count) { 61 | uint8_t data[] = {'l',led_count}; 62 | write(data, 2); 63 | } -------------------------------------------------------------------------------- /segbot_led/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | segbot_led 4 | 0.3.5 5 | 6 | LED package that provides drivers for communicating with Polulu WS2812B-Based LED Strips 7 | utilizing an Arduino microcontroller. Also action and service calls for standard applications. 8 | 9 | 10 | Pato Lankenau 11 | Rolando Fernandez. 12 | Piyush Khandelwal 13 | Jack O'Quin 14 | 15 | BSD 16 | 17 | catkin 18 | 19 | actionlib 20 | actionlib_msgs 21 | bwi_msgs 22 | roscpp 23 | serial 24 | std_msgs 25 | 26 | 27 | -------------------------------------------------------------------------------- /segbot_led/scripts/udev_install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | echo copying $(rospack find bwi_led)/udev to /etc/udev/rules.d 4 | /usr/bin/sudo /bin/cp -r $(rospack find bwi_led)/udev/* /etc/udev/rules.d 5 | echo reboot for changes to take effect -------------------------------------------------------------------------------- /segbot_led/udev/99-metromini.rules: -------------------------------------------------------------------------------- 1 | SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6015", ATTRS{serial}=="ADAPDYLsR", SYMLINK+="metromini" 2 | -------------------------------------------------------------------------------- /segbot_led/udev/README.md: -------------------------------------------------------------------------------- 1 | ### Configuring udev Rules 2 | 3 | Retrieve the attributes of the microcontroller to create the proper udev rule for the specific device. 4 | 5 | * You will need to monitor udev events to determine the serial port name for the device. Execute the command: 6 | 7 | ``` 8 | udevadm monitor 9 | ``` 10 | 11 | * Plug in the device and annotate what /dev/ name it was assigned. 12 | 13 | * Execute the command below and replace the question marks with the name given to your device. 14 | 15 | ``` 16 | udevadm info --attribute-walk --name=/dev/????? 17 | ``` 18 | 19 | * Retrive the follwing attribute values and replace the values in 99-metromini.rules. 20 | 21 | * ATTRS{idVendor} 22 | * ATTRS{idProduct} 23 | * ATTRS{serial} 24 | -------------------------------------------------------------------------------- /segbot_navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html 2 | cmake_minimum_required(VERSION 2.8.3) 3 | project(segbot_navigation) 4 | 5 | find_package(catkin REQUIRED COMPONENTS bwi_interruptable_action_server geometry_msgs move_base_msgs nav_msgs std_srvs) 6 | catkin_package( 7 | CATKIN_DEPENDS bwi_interruptable_action_server geometry_msgs move_base_msgs nav_msgs std_srvs 8 | ) 9 | 10 | include_directories(${catkin_INCLUDE_DIRS}) 11 | add_executable(move_base_interruptable_server src/move_base_interruptable_server.cpp) 12 | target_link_libraries(move_base_interruptable_server 13 | ${catkin_LIBRARIES} 14 | ) 15 | 16 | install(TARGETS move_base_interruptable_server 17 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 18 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 19 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 20 | ) 21 | 22 | foreach(dir config launch rviz) 23 | install(DIRECTORY ${dir}/ 24 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) 25 | endforeach() 26 | 27 | catkin_install_python(PROGRAMS scripts/move_base_interruptable_simple scripts/rviz_runner 28 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 29 | 30 | # Install the teleoperation script for rosrun 31 | install(PROGRAMS 32 | scripts/rviz_runner 33 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 34 | 35 | # unit tests are enabled selectively 36 | if (CATKIN_ENABLE_TESTING) 37 | find_package(roslaunch REQUIRED) 38 | roslaunch_add_file_check(launch/amcl.launch) 39 | roslaunch_add_file_check(launch/gmapping.launch) 40 | roslaunch_add_file_check(launch/move_base_eband.launch) 41 | roslaunch_add_file_check(launch/rviz.launch) 42 | ## launch/navigation.launch not tested until 43 | ## ros-drivers/openni_launch#10 is fixed. 44 | endif () 45 | -------------------------------------------------------------------------------- /segbot_navigation/config/segbotv2/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | robot_base_frame: base_footprint 2 | update_frequency: 3 | publish_frequency: 1.0 4 | inflation_radius: 1.0 5 | obstacle_layer/footprint_clearing_enabled: false 6 | 7 | # publish_voxel_map: true 8 | # 9 | # map_type: voxel 10 | # origin_z: 0.0 11 | # z_resolution: 0.2 12 | # z_voxels: 10 13 | # unknown_threshold: 10 14 | # mark_threshold: 0 15 | 16 | observation_sources: scan kinect_scan 17 | scan: {data_type: LaserScan, topic: scan_hallucinated, marking: true, clearing: true, inf_is_valid: true, obstacle_range: 3.75, raytrace_range: 4.0} 18 | kinect_scan: {data_type: LaserScan, topic: nav_kinect/scan_filtered, marking: true, clearing: true, inf_is_valid: true, obstacle_range: 2.75, raytrace_range: 2.95} #raytrace_range must be less than max range of sensor. 19 | -------------------------------------------------------------------------------- /segbot_navigation/config/segbotv2/eband_planner_params.yaml: -------------------------------------------------------------------------------- 1 | EBandPlannerROS: 2 | 3 | # TRAJECTORY CONTROL 4 | differential_drive: true 5 | 6 | # velocity limits 7 | max_vel_lin: 1.0 8 | max_vel_th: 0.5 9 | min_vel_lin: 0.4 10 | min_in_place_vel_th: 0.25 11 | 12 | # goal tolerance 13 | xy_goal_tolerance: 0.2 14 | yaw_goal_tolerance: 0.2 15 | 16 | # angular velocity PID 17 | k_prop: 0.75 18 | 19 | #eband parameters 20 | -------------------------------------------------------------------------------- /segbot_navigation/config/segbotv2/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | robot_base_frame: base_footprint 3 | update_frequency: 5.0 4 | publish_frequency: 5.0 5 | static_map: true 6 | transform_tolerance: 0.5 7 | footprint_padding: 0.05 8 | -------------------------------------------------------------------------------- /segbot_navigation/config/segbotv2/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: base_footprint 4 | update_frequency: 5.0 5 | publish_frequency: 5.0 6 | static_map: false 7 | rolling_window: true 8 | width: 8.0 9 | height: 8.0 10 | resolution: 0.05 11 | transform_tolerance: 0.25 12 | footprint_padding: 0.0 13 | -------------------------------------------------------------------------------- /segbot_navigation/config/segbotv2/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | footprint: [[0.225, 0.265], [0.225, -0.265], [-0.225, -0.265], [-0.34, 0], [-0.225, 0.265]] 2 | # robot_radius: 0.30 3 | planner_frequency: 1.0 4 | recovery_behaviors: [{name: rotate_recovery, type: rotate_recovery/RotateRecovery}] 5 | # recovery_behaviors: [{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, {name: rotate_recovery, type: rotate_recovery/RotateRecovery}] 6 | # conservative_reset_dist: 0.5 7 | clearing_radius: 0.0 8 | -------------------------------------------------------------------------------- /segbot_navigation/config/segbotv3/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | robot_base_frame: base_footprint 2 | update_frequency: 3 | publish_frequency: 1.0 4 | inflation_radius: 1.0 5 | 6 | # publish_voxel_map: true 7 | # 8 | # map_type: voxel 9 | # origin_z: 0.0 10 | # z_resolution: 0.2 11 | # z_voxels: 10 12 | # unknown_threshold: 10 13 | # mark_threshold: 0 14 | 15 | observation_sources: hokuyo_scan velodyne_cloud #velodyne_scan # Don't have both velodyne scan and velodyne cloud. 16 | hokuyo_scan: {data_type: LaserScan, topic: nav_hokuyo/scan, marking: true, clearing: true, inf_is_valid: true, obstacle_range: 3.75, raytrace_range: 4.0} #raytrace_range must be less than max range of sensor. 17 | velodyne_cloud: {data_type: PointCloud2, topic: velodyne/points_filtered, marking: true, clearing: true, obstacle_range: 3.75, raytrace_range: 4.00, max_obstacle_height: 1.5, min_obstacle_height: 0.1} #raytrace_range must be less than max range of sensor. 18 | #velodyne_scan: {data_type: PointCloud2, topic: velodyne/scan_filtered, marking: true, clearing: true, inf_is_valid: true, obstacle_range: 3.75, raytrace_range: 4.00} #raytrace_range must be less than max range of sensor. 19 | -------------------------------------------------------------------------------- /segbot_navigation/config/segbotv3/eband_planner_params.yaml: -------------------------------------------------------------------------------- 1 | EBandPlannerROS: 2 | 3 | # TRAJECTORY CONTROL 4 | differential_drive: true 5 | 6 | # velocity limits 7 | max_vel_lin: 1.0 8 | max_vel_th: 0.5 9 | min_vel_lin: 0.4 10 | min_in_place_vel_th: 0.25 11 | 12 | # goal tolerance 13 | xy_goal_tolerance: 0.2 14 | yaw_goal_tolerance: 0.2 15 | 16 | # angular velocity PID 17 | k_prop: 0.75 18 | 19 | #eband parameters 20 | -------------------------------------------------------------------------------- /segbot_navigation/config/segbotv3/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | robot_base_frame: base_footprint 3 | update_frequency: 5.0 4 | publish_frequency: 5.0 5 | static_map: true 6 | transform_tolerance: 0.5 7 | footprint_padding: 0.05 8 | -------------------------------------------------------------------------------- /segbot_navigation/config/segbotv3/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: base_footprint 4 | update_frequency: 5.0 5 | publish_frequency: 5.0 6 | static_map: false 7 | rolling_window: true 8 | width: 8.0 9 | height: 8.0 10 | resolution: 0.05 11 | transform_tolerance: 0.25 12 | footprint_padding: 0.0 13 | -------------------------------------------------------------------------------- /segbot_navigation/config/segbotv3/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | footprint: [[0.265, 0.285], [0.265, -0.285], [-0.265, -0.285], [-0.38, 0], [-0.265, 0.285]] 2 | 3 | planner_frequency: 1.0 4 | 5 | recovery_behaviors: 6 | - name: 'conservative_reset' 7 | type: 'clear_costmap_recovery/ClearCostmapRecovery' 8 | - name: 'rotate_recovery' 9 | type: 'rotate_recovery/RotateRecovery' 10 | - name: 'aggressive_reset' 11 | type: 'clear_costmap_recovery/ClearCostmapRecovery' 12 | 13 | conservative_reset: 14 | reset_distance: 3.0 15 | layer_names: ['obstacle_layer'] 16 | 17 | aggressive_reset: 18 | reset_distance: 0.0 19 | layer_names: ['obstacle_layer'] 20 | -------------------------------------------------------------------------------- /segbot_navigation/config/segbotv4/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | robot_base_frame: base_footprint 2 | update_frequency: 3 | publish_frequency: 1.0 4 | inflation_radius: 1.0 5 | obstacle_layer/footprint_clearing_enabled: false 6 | 7 | # publish_voxel_map: true 8 | # 9 | # map_type: voxel 10 | # origin_z: 0.0 11 | # z_resolution: 0.2 12 | # z_voxels: 10 13 | # unknown_threshold: 10 14 | # mark_threshold: 0 15 | 16 | observation_sources: hokuyo_scan 17 | hokuyo_scan: {data_type: LaserScan, topic: scan_filtered, marking: true, clearing: true, inf_is_valid: true, obstacle_range: 3.75, raytrace_range: 4.0} #raytrace_range must be less than max range of sensor. 18 | -------------------------------------------------------------------------------- /segbot_navigation/config/segbotv4/eband_planner_params.yaml: -------------------------------------------------------------------------------- 1 | EBandPlannerROS: 2 | 3 | # TRAJECTORY CONTROL 4 | differential_drive: true 5 | 6 | # velocity limits 7 | max_vel_lin: 1.0 8 | max_vel_th: 0.5 9 | min_vel_lin: 0.4 10 | min_in_place_vel_th: 0.25 11 | 12 | # goal tolerance 13 | xy_goal_tolerance: 0.2 14 | yaw_goal_tolerance: 0.2 15 | 16 | # angular velocity PID 17 | k_prop: 0.75 18 | 19 | #eband parameters 20 | eband_internal_force_gain: 3.0 21 | #eband_external_force_gain: 1.0 22 | 23 | costmap_weight: 15.0 24 | -------------------------------------------------------------------------------- /segbot_navigation/config/segbotv4/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | robot_base_frame: base_footprint 3 | update_frequency: 5.0 4 | publish_frequency: 5.0 5 | static_map: true 6 | transform_tolerance: 0.5 7 | footprint_padding: 0.05 8 | -------------------------------------------------------------------------------- /segbot_navigation/config/segbotv4/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: base_footprint 4 | update_frequency: 5.0 5 | publish_frequency: 5.0 6 | static_map: false 7 | rolling_window: true 8 | width: 8.0 9 | height: 8.0 10 | resolution: 0.05 11 | transform_tolerance: 0.25 12 | footprint_padding: 0.0 13 | -------------------------------------------------------------------------------- /segbot_navigation/config/segbotv4/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | footprint: [[0.265, 0.285], [0.265, -0.285], [-0.265, -0.285], [-0.38, 0], [-0.265, 0.285]] 2 | #robot_radius: 0.30 3 | planner_frequency: 1.0 4 | recovery_behaviors: [{name: rotate_recovery, type: rotate_recovery/RotateRecovery}] 5 | # recovery_behaviors: [{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, {name: rotate_recovery, type: rotate_recovery/RotateRecovery}] 6 | # conservative_reset_dist: 0.5 7 | clearing_radius: 0.0 8 | -------------------------------------------------------------------------------- /segbot_navigation/launch/README.md: -------------------------------------------------------------------------------- 1 | How to create a new map: 2 | 3 | 1) To create a new map, use the gmapping launch file for the version of BWI Bot you are using: 4 | 5 | For version 2 use the following: 6 | roslaunch segbot_navigation robot_with_gmapping_v2.launch 7 | 8 | For version 3 use the following: 9 | Before launching gmapping adjust the velodyne laser range max from 40 to 80 in the following launch file: 10 | segbot/segbot_sensors/launch/velodyne/velodyne-laserscan.launch 11 | 12 | roslaunch segbot_navigation robot_with_gmapping_v3.launch 13 | 14 | This will bring up the necessary tools to create the map and visualize it as it's being computed. 15 | 16 | In order to driver the robot around you must also run the keyboard teleoperation node: 17 | rosrun segbot_bringup teleop_twist_keyboard 18 | 19 | 20 | 2) Saving the map 21 | 22 | Once you are done with the map you can save it with the following command: 23 | rosrun map_server map_saver -f path/name 24 | 25 | NOTE!: This only works if gmapping (one of the tools launched by the first launch file) is still active! 26 | 27 | 28 | 3) Annotating the map 29 | 30 | To annotate the map you need to specify the path of the yaml file of the map, and a directory to store the data: 31 | 32 | rosrun bwi_planning_common logical_marker _map_file:=mapfile.yaml _data_directory:=aDirectory 33 | -------------------------------------------------------------------------------- /segbot_navigation/launch/gmapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /segbot_navigation/launch/gmapping_v3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /segbot_navigation/launch/navigation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /segbot_navigation/launch/navigation_v2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /segbot_navigation/launch/robot_with_gmapping_v2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /segbot_navigation/launch/robot_with_gmapping_v3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /segbot_navigation/launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 8 | -------------------------------------------------------------------------------- /segbot_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | segbot_navigation 3 | 0.3.5 4 | 5 | Contains launch files for running the ROS navigation stack on the segbot 6 | using the eband_local_planner approach, as well as launch files for amcl 7 | and gmapping. 8 | 9 | 10 | BSD 11 | 12 | http://ros.org/wiki/segbot_navigation 13 | https://github.com/utexas-bwi/segbot_apps.git 14 | https://github.com/utexas-bwi/segbot_apps/issues 15 | 16 | Piyush Khandelwal 17 | Piyush Khandelwal 18 | Jack O'Quin 19 | 20 | catkin 21 | 22 | bwi_interruptable_action_server 23 | geometry_msgs 24 | move_base_msgs 25 | nav_msgs 26 | std_srvs 27 | 28 | amcl 29 | eband_local_planner 30 | gmapping 31 | map_server 32 | move_base 33 | global_planner 34 | rviz 35 | segbot_bringup 36 | 37 | 38 | roslaunch 39 | 40 | 41 | -------------------------------------------------------------------------------- /segbot_navigation/scripts/move_base_interruptable_simple: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | 5 | # Brings in the SimpleActionClient 6 | import actionlib 7 | from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal 8 | from geometry_msgs.msg import PoseStamped 9 | 10 | def handle_move_base_simple_request(msg): 11 | # Creates the SimpleActionClient, passing the type of the action 12 | # (FibonacciAction) to the constructor. 13 | client = actionlib.SimpleActionClient('move_base_interruptable', MoveBaseAction) 14 | 15 | # Waits until the action server has started up and started 16 | # listening for goals. 17 | client.wait_for_server() 18 | 19 | # Creates a goal to send to the action server. 20 | goal = MoveBaseGoal(msg) 21 | 22 | # Sends the goal to the action server. 23 | client.send_goal(goal) 24 | 25 | if __name__ == '__main__': 26 | try: 27 | rospy.init_node('move_base_interruptable_simple') 28 | sub = rospy.Subscriber('move_base_interruptable_simple/goal', PoseStamped, handle_move_base_simple_request) 29 | rospy.spin() 30 | except rospy.ROSInterruptException: 31 | pass 32 | -------------------------------------------------------------------------------- /segbot_navigation/scripts/rviz_runner: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | 5 | tmpfile=`mktemp /tmp/tempfile.XXXXXXXXX` 6 | cp `rospack find segbot_navigation`/rviz/nav_eband_marvin.rviz $tmpfile 7 | if [ "$1" == "ns" ] 8 | then 9 | # A robot name has been specified. Simply replace the keyword marvin with supplied robot name. 10 | sed -i "s/marvin/$2/g" $tmpfile 11 | else 12 | # No robot name specified, run everything in top level namespace. 13 | sed -i "s/\/marvin//g" $tmpfile 14 | sed -i "s/marvin\///g" $tmpfile 15 | sed -i "s/marvin//g" $tmpfile 16 | fi 17 | $OPTIRUN_LAUNCH_PREFIX rosrun rviz rviz -d $tmpfile 18 | rm -f $tmpfile 19 | -------------------------------------------------------------------------------- /segbot_sensors/.gitignore: -------------------------------------------------------------------------------- 1 | *.py[cod] 2 | *~ 3 | .#* 4 | \#* 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Packages 10 | *.egg 11 | *.egg-info 12 | 13 | # Installer logs 14 | pip-log.txt 15 | 16 | # Unit test / coverage reports 17 | .coverage 18 | .tox 19 | nosetests.xml 20 | -------------------------------------------------------------------------------- /segbot_sensors/cfg/SegbotVelodyneOutlierRemoval.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | # set up parameters that we care about 4 | PACKAGE = 'segbot_sensors' 5 | 6 | from dynamic_reconfigure.parameter_generator_catkin import *; 7 | 8 | gen = ParameterGenerator () 9 | 10 | # enabling/disabling the unit limits 11 | # def add (self, name, paramtype, level, description, default = None, min = None, max = None, edit_method = ""): 12 | gen.add ("support_struct_angle", double_t, 0, "Angle between 0 and pi/4, representing cutoff point for support structure for support beam.", 0.6362, 0, 0.785398) 13 | gen.add ("wire_conduit_angle", double_t, 0, "Angle between 0 and pi/4, representing cutoff point induced by wire conduit. Should be typically slightly less than support_struct_angle", 0.6126, 0, 0.785398) 14 | gen.add ("range", double_t, 0, "Remove points up to this range.", 1.75, 0.4, 130.0) 15 | 16 | exit (gen.generate (PACKAGE, "segbot_sensors", "SegbotVelodyneOutlierRemoval")) 17 | 18 | -------------------------------------------------------------------------------- /segbot_sensors/config/diagnostics_analyzer.yaml: -------------------------------------------------------------------------------- 1 | #Holds the parameters for the rqt_monitor diagnostics 2 | 3 | pub_rate: 1.0 # Optional 4 | base_path: '' # Optional, prepended to all diagnostic output 5 | analyzers: 6 | sensors: #sets the namespace 7 | type: GenericAnalyzer 8 | path: Battery 9 | startswith: [ 10 | 'Remaining Battery Estimator', 11 | 'voltage' 12 | ] 13 | #computers: 14 | # type: GenericAnalyzer 15 | # path: Computers 16 | # contains: [ 17 | # 'HD Temp', 18 | # 'CPU Usage', 19 | # 'CPU Temperature', 20 | # 'HD Usage', 21 | # 'NFS'] 22 | -------------------------------------------------------------------------------- /segbot_sensors/config/hokuyo_filters.yaml: -------------------------------------------------------------------------------- 1 | scan_filter_chain: 2 | - name: nan_to_inf_filter 3 | type: segbot_sensors/NanToInfFilter 4 | - name: footprint_filter 5 | type: segbot_sensors/SegbotFootprintFilter 6 | params: 7 | footprint_frame: base_footprint 8 | footprint: [0.2,0.3,0.2,-0.3,-0.35,-0.25,-0.35,0.25] 9 | 10 | -------------------------------------------------------------------------------- /segbot_sensors/config/hokuyo_filters_v2.yaml: -------------------------------------------------------------------------------- 1 | scan_filter_chain: 2 | - name: shadows 3 | type: laser_filters/ScanShadowsFilter 4 | params: 5 | min_angle: 10 6 | max_angle: 170 7 | neighbors: 1 8 | window: 1 9 | - name: nan_to_inf_filter 10 | type: segbot_sensors/NanToInfFilter 11 | - name: footprint_filter 12 | type: segbot_sensors/SegbotFootprintFilter 13 | params: 14 | footprint_frame: base_footprint 15 | footprint: [0.2,0.3,0.2,-0.3,-0.35,-0.25,-0.35,0.25] 16 | 17 | -------------------------------------------------------------------------------- /segbot_sensors/config/kinect_filters.yaml: -------------------------------------------------------------------------------- 1 | scan_filter_chain: 2 | - name: nan_to_inf_filter 3 | type: segbot_sensors/NanToInfFilter 4 | -------------------------------------------------------------------------------- /segbot_sensors/config/v3_diagnostics_analyzer.yaml: -------------------------------------------------------------------------------- 1 | #Holds the parameters for the rqt_monitor diagnostics 2 | 3 | pub_rate: 1.0 # Optional 4 | base_path: '' # Optional, prepended to all diagnostic output 5 | 6 | analyzers: 7 | sensors: #sets the namespace 8 | type: GenericAnalyzer 9 | path: Battery 10 | startswith: [ 11 | 'battery_estimator', 12 | 'voltage' 13 | ] 14 | 15 | #computers: 16 | # type: GenericAnalyzer 17 | # path: Computers 18 | # contains: [ 19 | # 'HD Temp', 20 | # 'CPU Usage', 21 | # 'CPU Temperature', 22 | # 'HD Usage', 23 | # 'NFS'] 24 | -------------------------------------------------------------------------------- /segbot_sensors/config/velodyne_filters.yaml: -------------------------------------------------------------------------------- 1 | scan_filter_chain: 2 | # - name: nan_to_inf_filter 3 | # type: segbot_sensors/NanToInfFilter 4 | - name: shadows 5 | type: laser_filters/ScanShadowsFilter 6 | params: 7 | min_angle: 10 8 | max_angle: 170 9 | neighbors: 1 10 | window: 1 11 | -------------------------------------------------------------------------------- /segbot_sensors/launch/arduino/arduino.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /segbot_sensors/launch/diagnostic/diagnostics.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /segbot_sensors/launch/hokuyo/eth_hokuyo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /segbot_sensors/launch/hokuyo/hokuyo-filters.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /segbot_sensors/launch/hokuyo/hokuyo-frames.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | -------------------------------------------------------------------------------- /segbot_sensors/launch/hokuyo/hokuyo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /segbot_sensors/launch/hokuyo/urg_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /segbot_sensors/launch/kinect/kinect-filters.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /segbot_sensors/launch/kinect/kinect-laserscan.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /segbot_sensors/launch/ptgrey/usbcam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /segbot_sensors/launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /segbot_sensors/launch/velodyne/velodyne-filters.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /segbot_sensors/launch/velodyne/velodyne-laserscan.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /segbot_sensors/launch/velodyne/velodyne.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /segbot_sensors/launch/xtion.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /segbot_sensors/nodelets.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Creates PointCloud2 from a sensor_msgs/Range topic. 7 | 8 | 9 | 10 | 11 | 12 | 15 | 16 | SegbotVelodyneOutlierRemoval uses point neighborhood statistics to filter outlier data. 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /segbot_sensors/nodes/arduino_driver: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (C) 2015, Jack O'Quin 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the author nor of other contributors may be 18 | # used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | 34 | """ 35 | This Python script invokes the ROS device driver node for the Arduino 36 | Mega 2560 mounted on the BWI segbots (version 2). 37 | """ 38 | from segbot_sensors import arduino_driver_node 39 | 40 | if __name__ == '__main__': 41 | arduino_driver_node.main() 42 | -------------------------------------------------------------------------------- /segbot_sensors/segbot_sensors_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | This is a filter that removes points in a certain footprint. Useful for removing known laserscan hits on the body of the robot. 7 | 8 | 9 | 11 | 12 | This is a filter that converts nan points to +infinity, effectively making the assumption that locations where no readings are returned are free space. 13 | 14 | 15 | 17 | 18 | This is a filter that limits the angle range of the sensor. 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /segbot_sensors/setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['segbot_sensors'], 9 | package_dir={'': 'src'}) 10 | 11 | setup(**setup_args) 12 | -------------------------------------------------------------------------------- /segbot_sensors/src/segbot_sensors/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/utexas-bwi/segbot/1c6cc782d5c949fe11914a14a768789418b7dcd5/segbot_sensors/src/segbot_sensors/__init__.py -------------------------------------------------------------------------------- /segbot_sensors/src/segbot_sensors/fake_volt_pub.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/String.h" 3 | #include "sensor_msgs/BatteryState.h" 4 | #include 5 | 6 | int main(int argc, char **argv){ 7 | ros::init(argc, argv, "talker"); 8 | ros::NodeHandle n; 9 | ros::Publisher chatter_pub = n.advertise("/battery0", 1); 10 | ros::Rate loop_rate(10); 11 | 12 | int count = 0; 13 | while (ros::ok()){ 14 | sensor_msgs::BatteryState stat; 15 | stat.voltage = 10.9; 16 | chatter_pub.publish(stat); 17 | ros::spinOnce(); 18 | 19 | loop_rate.sleep(); 20 | } 21 | return 0; 22 | } 23 | -------------------------------------------------------------------------------- /segbot_sensors/src/segbot_sensors_filters.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LICENSE: https://github.com/utexas-bwi/segbot/blob/devel/LICENSE 3 | */ 4 | 5 | #include "segbot_sensors/footprint_filter.h" 6 | #include "segbot_sensors/nan_to_inf_filter.h" 7 | #include "segbot_sensors/angle_range_filter.h" 8 | #include "sensor_msgs/LaserScan.h" 9 | #include "filters/filter_base.h" 10 | 11 | #include "pluginlib/class_list_macros.h" 12 | 13 | PLUGINLIB_EXPORT_CLASS(segbot_sensors::SegbotFootprintFilter, filters::FilterBase) 14 | PLUGINLIB_EXPORT_CLASS(segbot_sensors::NanToInfFilter, filters::FilterBase) 15 | PLUGINLIB_EXPORT_CLASS(segbot_sensors::AngleRangeFilter, filters::FilterBase) 16 | -------------------------------------------------------------------------------- /segbot_sensors/src/sonar/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Conversions for sensor_msgs/Range messages 2 | # used for segbot version 2 3 | add_library(range_to_cloud range_nodelet.cpp range_to_cloud.cpp) 4 | target_link_libraries(range_to_cloud ${catkin_LIBRARIES}) 5 | install (TARGETS range_to_cloud 6 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 7 | -------------------------------------------------------------------------------- /segbot_sensors/test/imu_msg_test.txt: -------------------------------------------------------------------------------- 1 | I0 accel x: 18 y: -1000 z: 16000 2 | -------------------------------------------------------------------------------- /segbot_sensors/test/named-hokuyo-ns.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /segbot_sensors/test/named-kinect-ns.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /segbot_sensors/test/poll_cycle_arduino.txt: -------------------------------------------------------------------------------- 1 | 0=18cm 3=62cm 5=30cm 6=30cm 2 | 1=14cm 4=8cm 5=30cm 6=30cm 3 | 2=68cm 5=30cm 6=30cm 4 | 0=18cm 3=62cm 5=30cm 6=30cm 5 | 1=14cm 4=8cm 5=30cm 6=30cm 6 | 2=68cm 5=30cm 6=30cm 7 | 0=18cm 3=62cm 5=30cm 6=30cm 8 | 1=14cm 4=8cm 5=30cm 6=30cm 9 | 2=68cm 5=30cm 6=30cm 10 | 0=18cm 3=62cm 5=30cm 6=30cm 11 | 1=14cm 4=8cm 5=30cm 6=30cm 12 | 2=68cm 5=30cm 6=30cm 13 | 0=18cm 3=62cm 5=30cm 6=30cm 14 | 1=14cm 4=8cm 5=30cm 6=30cm 15 | 2=68cm 5=30cm 6=30cm 16 | 0=18cm 3=62cm 5=30cm 6=30cm 17 | 1=14cm 4=8cm 5=30cm 6=30cm 18 | 2=68cm 5=30cm 6=30cm 19 | 0=18cm 3=62cm 5=30cm 6=30cm 20 | 1=14cm 4=8cm 5=30cm 6=30cm 21 | 2=68cm 5=30cm 6=30cm 22 | 0=18cm 3=62cm 5=30cm 6=30cm 23 | 1=14cm 4=8cm 5=30cm 6=30cm 24 | 2=68cm 5=30cm 6=30cm 25 | 0=18cm 3=62cm 5=30cm 6=30cm 26 | 1=14cm 4=8cm 5=30cm 6=30cm 27 | 2=68cm 5=30cm 6=30cm 28 | 0=18cm 3=62cm 5=30cm 6=30cm 29 | 1=14cm 4=8cm 5=30cm 6=30cm 30 | 2=68cm 5=30cm 6=30cm 31 | 0=18cm 3=62cm 5=30cm 6=30cm 32 | 1=14cm 4=8cm 5=30cm 6=30cm 33 | 2=68cm 5=30cm 6=30cm 34 | 0=18cm 3=62cm 5=30cm 6=30cm 35 | 1=14cm 4=8cm 5=30cm 6=30cm 36 | 2=68cm 5=30cm 6=30cm 37 | 0=18cm 3=62cm 5=30cm 6=30cm 38 | 1=14cm 4=8cm 5=30cm 6=30cm 39 | 2=68cm 5=30cm 6=30cm 40 | 0=18cm 3=62cm 5=30cm 6=30cm 41 | 1=14cm 4=8cm 5=30cm 6=30cm 42 | 2=68cm 5=30cm 6=30cm 43 | 0=18cm 3=62cm 5=30cm 6=30cm 44 | 1=14cm 4=8cm 5=30cm 6=30cm 45 | 2=68cm 5=30cm 6=30cm 46 | 0=18cm 3=62cm 5=30cm 6=30cm 47 | 1=14cm 4=8cm 5=30cm 6=30cm 48 | 2=68cm 5=30cm 6=30cm 49 | 0=18cm 3=62cm 5=30cm 6=30cm 50 | 1=14cm 4=8cm 5=30cm 6=30cm 51 | 2=68cm 5=30cm 6=30cm 52 | 0=18cm 3=62cm 5=30cm 6=30cm 53 | 1=14cm 4=8cm 5=30cm 6=30cm 54 | 2=68cm 5=30cm 6=30cm 55 | 0=18cm 3=62cm 5=30cm 6=30cm 56 | 1=14cm 4=8cm 5=30cm 6=30cm 57 | 2=68cm 5=30cm 6=30cm 58 | -------------------------------------------------------------------------------- /segbot_sensors/test/sonar_msg_test.txt: -------------------------------------------------------------------------------- 1 | S0=18cm 2 | S1=14cm 3 | S2=68cm 4 | S0=18cm 5 | S1=14cm 6 | S2=68cm 7 | S0=18cm 8 | S1=14cm 9 | S2=68cm 10 | S0=18cm 11 | S1=14cm 12 | S2=68cm 13 | S0=18cm 14 | S1=14cm 15 | S2=68cm 16 | S0=18cm 17 | S1=14cm 18 | S2=68cm 19 | S0=18cm 20 | S1=14cm 21 | S2=68cm 22 | S0=18cm 23 | S1=14cm 24 | S2=68cm 25 | S0=18cm 26 | S1=14cm 27 | S2=68cm 28 | S0=18cm 29 | S1=14cm 30 | S2=68cm 31 | S0=18cm 32 | S1=14cm 33 | S2=68cm 34 | S0=18cm 35 | S1=14cm 36 | S2=68cm 37 | S0=18cm 38 | S1=14cm 39 | S2=68cm 40 | S0=18cm 41 | S1=14cm 42 | S2=68cm 43 | S0=18cm 44 | S1=14cm 45 | S2=68cm 46 | S0=18cm 47 | S1=14cm 48 | S2=68cm 49 | S0=18cm 50 | S1=14cm 51 | S2=68cm 52 | S0=18cm 53 | S1=14cm 54 | S2=68cm 55 | S0=18cm 56 | S1=14cm 57 | S2=68cm 58 | -------------------------------------------------------------------------------- /segbot_sensors/test/test_arduino.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 21 | 22 | 25 | 28 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /segbot_sensors/test/voltage_msg_test.txt: -------------------------------------------------------------------------------- 1 | V13.21 2 | -------------------------------------------------------------------------------- /segbot_simulation_apps/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package segbot_simulation_apps 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.5 (2016-08-27) 6 | ------------------ 7 | 8 | 0.3.4 (2016-08-08) 9 | ------------------ 10 | * added some code to remove redundant ros timers from the timeout queue. fixed incorrect footprint clearing. 11 | * updated simulated calls that open the doors to automatically close those doors. 12 | * finished + tested compilation of robot teleportation script. 13 | * separated some common functionality in door handler so that it can be used by robot teleporter. 14 | * added some stuff for the robot teleporter 15 | * Contributors: Piyush Khandelwal 16 | 17 | 0.3.3 (2015-08-05) 18 | ------------------ 19 | * merge segbot_simulator packages into segbot (`#46 `_) 20 | * Contributors: Jack O'Quin 21 | 22 | 0.3.1 (2015-03-31) 23 | ------------------ 24 | 25 | 0.3.0 (2015-03-24) 26 | ------------------ 27 | * added missing dependency on multi_level_map_msgs. 28 | * migrated DoorHandlerInterface from segbot_simulation_apps to bwi_msgs in the bwi_common repository. 29 | * updated door handler to use new multimap. 30 | * Contributors: Piyush Khandelwal 31 | 32 | 0.2.1 (2014-04-22) 33 | ------------------ 34 | 35 | 0.2.0 (2014-04-20) 36 | ------------------ 37 | * initial release to Hydro 38 | * install header correctly 39 | * removed catkin_lint errors 40 | * moved resolveDoor to common functions in bwi_planning_common 41 | * added service to modify door status 42 | * added a package for simulation apps. For now, added a door handler. 43 | -------------------------------------------------------------------------------- /segbot_simulation_apps/include/segbot_simulation_apps/common.h: -------------------------------------------------------------------------------- 1 | #ifndef SEGBOT_SIM_APPS_COMMON_H 2 | #define SEGBOT_SIM_APPS_COMMON_H 3 | 4 | #include 5 | 6 | namespace segbot_simulation_apps { 7 | 8 | bool checkClosePoses(const geometry_msgs::Pose& p1, 9 | const geometry_msgs::Pose& p2, 10 | float threshold = 0.05, 11 | bool check_yaw = true); 12 | 13 | bool teleportEntity(const std::string& entity, 14 | const geometry_msgs::Pose& pose, 15 | ros::ServiceClient& get_gazebo_model_client, 16 | ros::ServiceClient& set_gazebo_model_client); 17 | 18 | } /* segbot_simulation_apps */ 19 | 20 | 21 | #endif /* end of include guard: SEGBOT_SIM_APPS_COMMON_H */ 22 | -------------------------------------------------------------------------------- /segbot_simulation_apps/launch/door_handler.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /segbot_simulation_apps/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | segbot_simulation_apps 4 | 0.3.5 5 | 6 | Applications designed specifically to be used in a simulation 7 | environment, such as opening and closing doors inside the building 8 | simulation. 9 | 10 | 11 | Piyush Khandelwal 12 | Jack O'Quin 13 | 14 | BSD 15 | 16 | http://wiki.ros.org/segbot_simulation_apps 17 | https://github.com/utexas-bwi/segbot_simulator.git 18 | https://github.com/utexas-bwi/segbot_simulator/issues 19 | 20 | Piyush Khandelwal 21 | 22 | catkin 23 | 24 | boost 25 | bwi_msgs 26 | bwi_planning_common 27 | bwi_tools 28 | gazebo_msgs 29 | geometry_msgs 30 | multi_level_map_msgs 31 | roscpp 32 | std_msgs 33 | tf 34 | 35 | 36 | roslaunch 37 | 38 | 39 | -------------------------------------------------------------------------------- /segbot_simulation_apps/urdf/door.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | Gazebo/Red 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /segbot_simulation_apps/urdf/obstacle.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | Gazebo/Blue 45 | 46 | 47 | 48 | --------------------------------------------------------------------------------