├── README.md ├── fetch_bringup ├── CHANGELOG.rst ├── CMakeLists.txt ├── config │ ├── default_controllers.yaml │ └── graft.yaml ├── launch │ ├── fetch.launch │ └── include │ │ ├── graft.launch.xml │ │ ├── head_camera.launch.xml │ │ ├── laser.launch.xml │ │ ├── runstop.launch.xml │ │ └── teleop.launch.xml ├── package.xml └── scripts │ ├── controller_reset.py │ ├── software_runstop.py │ └── test │ └── test_software_runstop.py └── freight_bringup ├── CHANGELOG.rst ├── CMakeLists.txt ├── config ├── default_controllers.yaml └── graft.yaml ├── launch ├── freight.launch ├── freight_r1.launch └── include │ ├── base_camera_pmd.launch.xml │ ├── graft.launch.xml │ ├── laser.launch.xml │ └── teleop.launch.xml └── package.xml /README.md: -------------------------------------------------------------------------------- 1 | # fetch_robots 2 | 3 | Bringup files for Fetch and Freight. -------------------------------------------------------------------------------- /fetch_bringup/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package fetch_bringup 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.7.2 (2016-06-14) 6 | ------------------ 7 | 8 | 0.7.1 (2016-05-05) 9 | ------------------ 10 | * update to use /dev/ps3joy 11 | * Contributors: Michael Ferguson 12 | 13 | 0.7.0 (2016-03-28) 14 | ------------------ 15 | * fix dependency issue with sixad 16 | * Contributors: Michael Ferguson 17 | 18 | 0.6.1 (2016-03-22) 19 | ------------------ 20 | * require latest sixad 21 | * Add autorepeat_rate parameter to teleop launch 22 | * Contributors: Michael Ferguson, Michael Hwang 23 | 24 | 0.6.0 (2015-06-23) 25 | ------------------ 26 | * start tuck_arm.py as joystick teleop 27 | * disable gripper when resetting controllers 28 | * Contributors: Michael Ferguson 29 | 30 | 0.5.5 (2015-05-21) 31 | ------------------ 32 | * use no_delay parameter with graft 33 | * Contributors: Michael Ferguson 34 | 35 | 0.5.4 (2015-05-10) 36 | ------------------ 37 | * filter shadow points from laser 38 | * reorganize launch files for easier updating of calibrated robots 39 | * Contributors: Michael Ferguson 40 | 41 | 0.5.3 (2015-05-03) 42 | ------------------ 43 | * use new laser safety feature of base controller 44 | * add firmware param for gripper 45 | * Contributors: Michael Ferguson 46 | 47 | 0.5.2 (2015-04-19) 48 | ------------------ 49 | * hold position when stopped 50 | * add default calibration_date 51 | * Contributors: Michael Ferguson 52 | 53 | 0.5.1 (2015-04-09) 54 | ------------------ 55 | * add controller reset script 56 | * gas spring replaces gravity comp 57 | * added debug flag for fetch drivers 58 | * Contributors: Aaron Blasdel, Michael Ferguson 59 | 60 | 0.5.0 (2015-04-04) 61 | ------------------ 62 | 63 | 0.4.2 (2015-03-23) 64 | ------------------ 65 | * add depend on joy 66 | * Contributors: Michael Ferguson 67 | 68 | 0.4.1 (2015-03-23) 69 | ------------------ 70 | 71 | 0.4.0 (2015-03-22) 72 | ------------------ 73 | * update to use fetch_teleop 74 | * Contributors: Michael Ferguson 75 | 76 | 0.3.2 (2015-03-21) 77 | ------------------ 78 | * specify minimum version of laser drivers 79 | * fix graft launch file include 80 | * add head camera calibration 81 | * update laser parameters 82 | * Contributors: Michael Ferguson 83 | 84 | 0.3.1 (2015-03-13 19:53) 85 | ------------------------ 86 | 87 | 0.3.0 (2015-03-13 18:59) 88 | ------------------------ 89 | * first release 90 | * Contributors: Michael Ferguson 91 | -------------------------------------------------------------------------------- /fetch_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(fetch_bringup) 3 | 4 | find_package(catkin) 5 | catkin_package() 6 | 7 | install( 8 | DIRECTORY config 9 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 10 | ) 11 | 12 | install( 13 | DIRECTORY launch 14 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 15 | ) 16 | 17 | install( 18 | PROGRAMS scripts/controller_reset.py 19 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 20 | ) 21 | 22 | install( 23 | PROGRAMS scripts/software_runstop.py 24 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 25 | ) 26 | -------------------------------------------------------------------------------- /fetch_bringup/config/default_controllers.yaml: -------------------------------------------------------------------------------- 1 | arm_controller: 2 | follow_joint_trajectory: 3 | type: "robot_controllers/FollowJointTrajectoryController" 4 | joints: 5 | - shoulder_pan_joint 6 | - shoulder_lift_joint 7 | - upperarm_roll_joint 8 | - elbow_flex_joint 9 | - forearm_roll_joint 10 | - wrist_flex_joint 11 | - wrist_roll_joint 12 | gravity_compensation: 13 | type: "robot_controllers/GravityCompensation" 14 | root: "torso_lift_link" 15 | tip: "gripper_link" 16 | autostart: true 17 | 18 | arm_with_torso_controller: 19 | follow_joint_trajectory: 20 | type: "robot_controllers/FollowJointTrajectoryController" 21 | joints: 22 | - torso_lift_joint 23 | - shoulder_pan_joint 24 | - shoulder_lift_joint 25 | - upperarm_roll_joint 26 | - elbow_flex_joint 27 | - forearm_roll_joint 28 | - wrist_flex_joint 29 | - wrist_roll_joint 30 | 31 | torso_controller: 32 | follow_joint_trajectory: 33 | type: "robot_controllers/FollowJointTrajectoryController" 34 | joints: 35 | - torso_lift_joint 36 | 37 | head_controller: 38 | point_head: 39 | type: "robot_controllers/PointHeadController" 40 | follow_joint_trajectory: 41 | type: "robot_controllers/FollowJointTrajectoryController" 42 | joints: 43 | - head_pan_joint 44 | - head_tilt_joint 45 | 46 | base_controller: 47 | type: "robot_controllers/DiffDriveBaseController" 48 | max_velocity_x: 1.0 49 | max_acceleration_x: 0.75 50 | # hold position 51 | moving_threshold: -0.01 52 | rotating_threshold: -0.01 53 | # autostart to get odom 54 | autostart: true 55 | # use laser to only slowly collide with things 56 | laser_safety_dist: 1.0 57 | 58 | robot_driver: 59 | default_controllers: 60 | - "arm_controller/follow_joint_trajectory" 61 | - "arm_controller/gravity_compensation" 62 | - "arm_with_torso_controller/follow_joint_trajectory" 63 | - "base_controller" 64 | - "head_controller/follow_joint_trajectory" 65 | - "head_controller/point_head" 66 | - "torso_controller/follow_joint_trajectory" 67 | -------------------------------------------------------------------------------- /fetch_bringup/config/graft.yaml: -------------------------------------------------------------------------------- 1 | planar_output: True # Output only x, y, and rotation about z 2 | 3 | output_frame: odom # TF frame id, param name ported from robot_pose_ekf 4 | parent_frame_id: odom # TF frame id, override output_frame if set 5 | child_frame_id: base_link #TF frame id 6 | publish_tf: true 7 | 8 | update_rate: 50.0 9 | update_topic: odom 10 | 11 | queue_size: 1 12 | 13 | # Filter parameters 14 | alpha: 0.001 15 | kappa: 0.0 16 | beta: 2.0 17 | 18 | # Process noise covariance 2x2 for velocites, 5x5 for 2d position 19 | process_noise: [1e6, 0, 0, 20 | 0, 1e6, 0, 21 | 0, 0, 1e6] 22 | 23 | # Inputs 24 | topics: { 25 | odom: { 26 | topic: /odom, 27 | type: nav_msgs/Odometry, 28 | absolute_pose: False, 29 | use_velocities: True, 30 | timeout: 1.0, 31 | no_delay: True, 32 | override_twist_covariance: [1e-3, 0, 0, 0, 0, 0, 33 | 0, 1e-3, 0, 0, 0, 0, 34 | 0, 0, 1e-6, 0, 0, 0, 35 | 0, 0, 0, 1e-6, 0, 0, 36 | 0, 0, 0, 0, 1e-6, 0, 37 | 0, 0, 0, 0, 0, 1e-3], 38 | }, 39 | 40 | imu: { 41 | topic: /imu, 42 | type: sensor_msgs/Imu, 43 | absolute_orientation: False, 44 | use_velocities: True, 45 | use_accelerations: False, 46 | timeout: 1.0, 47 | no_delay: True, 48 | override_angular_velocity_covariance: [0, 0, 0, 49 | 0, 0, 0, 50 | 0, 0, 0.000004], 51 | }, 52 | } 53 | -------------------------------------------------------------------------------- /fetch_bringup/launch/fetch.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 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 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /fetch_bringup/launch/include/graft.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /fetch_bringup/launch/include/head_camera.launch.xml: -------------------------------------------------------------------------------- 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 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /fetch_bringup/launch/include/laser.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /fetch_bringup/launch/include/runstop.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /fetch_bringup/launch/include/teleop.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /fetch_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | fetch_bringup 3 | 0.7.2 4 | 5 | Bringup for fetch 6 | 7 | Michael Ferguson 8 | Michael Ferguson 9 | Proprietary 10 | catkin 11 | depth_image_proc 12 | fetch_description 13 | fetch_drivers 14 | fetch_teleop 15 | graft 16 | image_proc 17 | joy 18 | openni2_launch 19 | robot_controllers_msgs 20 | robot_state_publisher 21 | sensor_msgs 22 | sick_tim 23 | sixad 24 | 25 | -------------------------------------------------------------------------------- /fetch_bringup/scripts/controller_reset.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2015, Fetch Robotics Inc. 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | # 9 | # * Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # * Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in the 13 | # documentation and/or other materials provided with the distribution. 14 | # * Neither the name of the Fetch Robotics Inc. nor the names of its 15 | # contributors may be used to endorse or promote products derived from 16 | # this software without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | # ARE DISCLAIMED. IN NO EVENT SHALL FETCH ROBOTICS INC. BE LIABLE FOR ANY 22 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 27 | # THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | # Author: Michael Ferguson 30 | 31 | import rospy 32 | import actionlib 33 | from actionlib_msgs.msg import GoalStatus 34 | from control_msgs.msg import GripperCommandAction, GripperCommandGoal 35 | from sensor_msgs.msg import Joy 36 | from robot_controllers_msgs.msg import QueryControllerStatesAction, \ 37 | QueryControllerStatesGoal, \ 38 | ControllerState 39 | 40 | # Listen to joy messages, when button held, reset controllers 41 | class ControllerResetTeleop: 42 | CONTROLLER_ACTION_NAME = "/query_controller_states" 43 | GRIPPER_ACTION_NAME = "/gripper_controller/gripper_action" 44 | 45 | def __init__(self): 46 | rospy.loginfo("Connecting to %s..." % self.CONTROLLER_ACTION_NAME) 47 | self.controller_client = actionlib.SimpleActionClient(self.CONTROLLER_ACTION_NAME, QueryControllerStatesAction) 48 | self.controller_client.wait_for_server() 49 | rospy.loginfo("Done.") 50 | 51 | rospy.loginfo("Connecting to %s..." % self.GRIPPER_ACTION_NAME) 52 | self.gripper_client = actionlib.SimpleActionClient(self.GRIPPER_ACTION_NAME, GripperCommandAction) 53 | 54 | self.start = list() 55 | self.start.append("arm_controller/gravity_compensation") 56 | 57 | self.stop = list() 58 | self.stop.append("arm_controller/follow_joint_trajectory") 59 | self.stop.append("arm_with_torso_controller/follow_joint_trajectory") 60 | self.stop.append("torso_controller/follow_joint_trajectory") 61 | self.stop.append("head_controller/follow_joint_trajectory") 62 | self.stop.append("head_controller/point_head") 63 | 64 | self.reset_button = rospy.get_param("~reset_button", 4) # default button is the up button 65 | 66 | self.pressed = False 67 | self.pressed_last = None 68 | 69 | self.joy_sub = rospy.Subscriber("joy", Joy, self.joy_callback) 70 | 71 | def joy_callback(self, msg): 72 | try: 73 | if msg.buttons[self.reset_button] > 0: 74 | if not self.pressed: 75 | self.pressed_last = rospy.Time.now() 76 | self.pressed = True 77 | elif self.pressed_last and rospy.Time.now() > self.pressed_last + rospy.Duration(1.0): 78 | self.reset() 79 | self.pressed_last = None 80 | else: 81 | self.pressed = False 82 | except KeyError: 83 | rospy.logwarn("reset_button is out of range") 84 | 85 | def reset(self): 86 | # Reset controllers 87 | goal = QueryControllerStatesGoal() 88 | 89 | for controller in self.start: 90 | state = ControllerState() 91 | state.name = controller 92 | state.state = state.RUNNING 93 | goal.updates.append(state) 94 | 95 | for controller in self.stop: 96 | state = ControllerState() 97 | state.name = controller 98 | state.state = state.STOPPED 99 | goal.updates.append(state) 100 | 101 | self.controller_client.send_goal(goal) 102 | 103 | # Disable gripper torque 104 | goal = GripperCommandGoal() 105 | goal.command.max_effort = -1.0 106 | self.gripper_client.send_goal(goal) 107 | 108 | if __name__ == "__main__": 109 | rospy.init_node("controller_reset") 110 | c = ControllerResetTeleop() 111 | rospy.spin() 112 | -------------------------------------------------------------------------------- /fetch_bringup/scripts/software_runstop.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2016, Fetch Robotics Inc. 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | # 9 | # * Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # * Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in the 13 | # documentation and/or other materials provided with the distribution. 14 | # * Neither the name of the Fetch Robotics Inc. nor the names of its 15 | # contributors may be used to endorse or promote products derived from 16 | # this software without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | # ARE DISCLAIMED. IN NO EVENT SHALL FETCH ROBOTICS INC. BE LIABLE FOR ANY 22 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 27 | # THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | # Author: Eric Relson 30 | 31 | # Script launches a ROS node which can disable breakers. Node can be 32 | # controlled via the /enable_software_runstop topic. 33 | # Script is launched via e.g. teleop.launch.xml (or can be run by itself.) 34 | # 35 | # With the tele option the right two trigger buttons simultaneously pressed 36 | # will cause breakers to be disabled on the robot. 37 | # Software runstop is canceled by toggling the physical runstop (assuming the 38 | # controller buttons have been released). 39 | # 40 | # Note that if script is ended while software runstop is enabled, script 41 | # will need to be run again and controller buttons pressed again before 42 | # physical runstop will disable software runstop, due to breaker states set via 43 | # power_msgs persisting. 44 | 45 | # Standard Library 46 | from argparse import ArgumentParser 47 | 48 | # Third Party 49 | import rospy 50 | from sensor_msgs.msg import Joy 51 | from std_msgs.msg import Bool 52 | 53 | # Fetch 54 | from fetch_driver_msgs.msg import RobotState 55 | from power_msgs.srv import BreakerCommand 56 | 57 | 58 | # Listen to joy messages, when button held, reset controllers 59 | class SoftwareRunstop(object): 60 | """Monitor /Joy and /RobotState to implement a software runstop. 61 | 62 | Parameters 63 | ---------- 64 | arm, base, gripper : bools 65 | If True, corresponding breaker will be disabled by software runstop. 66 | tele : bool 67 | If True, will create a subscriber to /Joy which will monitor for 68 | button presses triggering the software runstop. 69 | 70 | Attributes 71 | ---------- 72 | hw_runstop_has_been_pressed : bool 73 | Records if hardware runstop has been pressed while software runstop 74 | was enabled. Set by update(). 75 | sw_runstop_enabled : bool 76 | If True, then the software runstop is enabled. 77 | Set by update(). 78 | hw_runstop_state : bool 79 | True = hardware runstop is pressed. 80 | Set by /robot_state callback. 81 | enable_breakers : bool 82 | If True, will disable software runstop and re-power breakers. 83 | Set by update() or by callback for /enable_software_runstop. 84 | disable_breakers : bool 85 | If True, will enable software runstop and disable the breakers. 86 | Set by update() based on /Joy and /enable_software_runstop callback 87 | methods. 88 | disable_breakers_tele : bool 89 | Indicates that controller wants breakers to be disabled (i.e. 90 | enable the software runstop). 91 | Requires the --tele option for related subscriber to be created. 92 | Set by joy_callback; disabled by update(). 93 | disable_breakers_ext : bool 94 | Indicates that /enable_software_runstop topic wants the breakers to be 95 | disabled (i.e. enable the software runstop). 96 | Set by /enable_topic_callback; disabled by update(). 97 | desired_breaker_states : dict of bools 98 | Stores the desired breaker state (True = on) for all breakers being 99 | controlled. 100 | """ 101 | def __init__(self, arm, base, gripper, tele): 102 | if not any([arm, base, gripper]): 103 | raise ValueError("SoftwareRunstop invoked without any " 104 | "breakers to be controlled.") 105 | 106 | self._check_robot_components() 107 | 108 | self.desired_breaker_states = {} 109 | if arm and self._has_arm: 110 | self.desired_breaker_states["/arm_breaker"] = True 111 | if base and self._has_base: 112 | self.desired_breaker_states["/base_breaker"] = True 113 | # Gripper requiring arm is same assumption used in drivers 114 | if gripper and self._has_arm: 115 | self.desired_breaker_states["/gripper_breaker"] = True 116 | self.tele = tele 117 | 118 | # Booleans; See class docstring 119 | self.hw_runstop_has_been_pressed = False 120 | self.sw_runstop_enabled = False 121 | self.hw_runstop_state = False 122 | # These two store whether we want to enable/disable breakers 123 | self.enable_breakers = False 124 | self.disable_breakers = False 125 | self.disable_breakers_tele = False 126 | self.disable_breakers_ext = False 127 | 128 | def _check_robot_components(self): 129 | """Check ROS params""" 130 | self._has_arm = rospy.get_param('robot_driver/has_arm', False) 131 | self._has_base = rospy.get_param('robot_driver/has_base', False) 132 | 133 | def setup_and_run(self): 134 | """Connect to topics and start running software runstop""" 135 | # Buttons 136 | self.deadman_button_a = rospy.get_param( 137 | "~deadman_button_a", 11) # default is top right trigger 138 | self.deadman_button_b = rospy.get_param( 139 | "~deadman_button_b", 9) # default is bottom right trigger 140 | 141 | # Subscribers 142 | self.robot_state_sub = rospy.Subscriber( 143 | "/robot_state", RobotState, self.robot_state_callback) 144 | self.enable_sub = rospy.Subscriber( 145 | "/enable_software_runstop", Bool, self.enable_topic_callback) 146 | if self.tele: 147 | self.joy_sub = rospy.Subscriber("/joy", Joy, self.joy_callback) 148 | 149 | # Publisher to topic /software_runstop_enabled 150 | self.pub = rospy.Publisher( 151 | "/software_runstop_enabled", Bool, queue_size=1) 152 | 153 | # Run 154 | self.r = rospy.Rate(10) 155 | self.run_node() 156 | 157 | 158 | def joy_callback(self, msg): 159 | """Callback for Joy messages published by teleop""" 160 | try: 161 | if (msg.buttons[self.deadman_button_a] > 0 and 162 | msg.buttons[self.deadman_button_b] > 0): 163 | self.disable_breakers_tele = True 164 | else: 165 | self.disable_breakers_tele = False 166 | except KeyError: 167 | rospy.logwarn("deadman_button values may be out of range") 168 | 169 | 170 | def enable_topic_callback(self, msg): 171 | """Callback for requests to enable software runstop""" 172 | if msg.data: 173 | self.disable_breakers_ext = True 174 | else: 175 | self.enable_breakers = True 176 | 177 | 178 | def robot_state_callback(self, msg): 179 | """Check hardware runstop status if software runstop is enabled""" 180 | if msg.runstopped: 181 | self.hw_runstop_state = True 182 | else: 183 | self.hw_runstop_state = False 184 | 185 | 186 | def update(self): 187 | """Primary logic for enabling/disabling software runstop 188 | 189 | Logic uses five booleans to track controller and runstop and the 190 | next step to take. In normal operation, everything is False: 191 | 192 | - self.hw_runstop_has_been_pressed 193 | - self.sw_runstop_enabled 194 | - self.hw_runstop_state 195 | - self.enable_breakers 196 | - self.disable_breakers 197 | """ 198 | # Handle external inputs that can disable breakers 199 | if self.disable_breakers_ext or self.disable_breakers_tele: 200 | self.disable_breakers = True 201 | self.disable_breakers_ext = False 202 | self.disable_breakers_tele = False 203 | # Want to disable breakers 204 | for breaker in self.desired_breaker_states: 205 | self.desired_breaker_states[breaker] = False 206 | else: 207 | # Want to enable breakers 208 | for breaker in self.desired_breaker_states: 209 | self.desired_breaker_states[breaker] = True 210 | 211 | # If want to disable breakers and sw runstop not already active 212 | if self.disable_breakers and not self.sw_runstop_enabled: 213 | # Disable the breakers; software runstop is active 214 | if all(self.set_breaker(breaker, val) for breaker, val in 215 | self.desired_breaker_states.items()): 216 | self.disable_breakers = False 217 | self.sw_runstop_enabled = True 218 | # If receive disable signal and software runstop is already active, 219 | elif self.disable_breakers and self.sw_runstop_enabled: 220 | # Clear disable_breakers and also enable_breakers 221 | self.disable_breakers = False 222 | self.enable_breakers = False 223 | # Elif want to enable breakers and software runstop is enabled 224 | # and hw runstop is not currently engaged 225 | elif self.enable_breakers and self.sw_runstop_enabled and \ 226 | not self.hw_runstop_state: 227 | # No longer want software runstop; turn breakers back on 228 | if all(self.set_breaker(breaker, val) for breaker, val in 229 | self.desired_breaker_states.items()): 230 | self.enable_breakers = False 231 | self.sw_runstop_enabled = False 232 | # If software runstopped, and waiting for hw runstop to be toggled 233 | elif self.sw_runstop_enabled: 234 | # If hw runstop is currently pressed 235 | if self.hw_runstop_state: 236 | self.hw_runstop_has_been_pressed = True 237 | # Else if hw runstop is released and was previously pressed since 238 | # software runstop was activated 239 | elif self.hw_runstop_has_been_pressed: 240 | self.enable_breakers = True 241 | self.hw_runstop_has_been_pressed = False 242 | # Else continue waiting for hw runstop action 243 | # Clear enable_breakers if software runstop already disabled 244 | elif not self.sw_runstop_enabled and self.enable_breakers: 245 | self.enable_breakers = False 246 | # Not actively disabling or enabling breaker, or waiting for hw runstop 247 | else: 248 | pass 249 | 250 | 251 | def run_node(self): 252 | """Run update function and publish software runstop state""" 253 | while not rospy.is_shutdown(): 254 | self.update() 255 | self.pub.publish(self.sw_runstop_enabled) 256 | # For debugging if issues arise with an invalid state, etc. 257 | #print("{}\t{}\t{}\t{}\t{}".format(self.enable_breakers, self.disable_breakers, self.sw_runstop_enabled, self.hw_runstop_state, self.hw_runstop_has_been_pressed)) 258 | self.r.sleep() 259 | 260 | 261 | def set_breaker(self, service_name, enable): 262 | """Toggle a breaker via power_msgs service 263 | 264 | Returns 265 | ------- 266 | bool 267 | False if a service call failed. Otherwise True. 268 | """ 269 | rospy.loginfo("Software runstop waiting for %s service...", service_name) 270 | rospy.wait_for_service(service_name) 271 | try: 272 | breaker = rospy.ServiceProxy(service_name, BreakerCommand) 273 | resp = breaker(enable) 274 | rospy.loginfo("Server sent response : \n" + str(resp)) 275 | except rospy.ServiceException, e: 276 | rospy.logerr("Service call failed: %s" % e) 277 | return False 278 | return True 279 | 280 | 281 | if __name__ == "__main__": 282 | parser = ArgumentParser() 283 | parser.add_argument("-a", "--arm", action="store_true", 284 | help="Software runstop will disable arm breaker.") 285 | parser.add_argument("-b", "--base", action="store_true", 286 | help="Software runstop will disable base breaker.") 287 | parser.add_argument("-g", "--gripper", action="store_true", 288 | help="Software runstop will disable gripper breaker.") 289 | parser.add_argument("-t", "--tele", action="store_true", help="Allow " 290 | "controller to trigger software runstop.") 291 | 292 | args, unknown = parser.parse_known_args() 293 | 294 | rospy.init_node("software_runstop") 295 | c = SoftwareRunstop(args.arm, args.base, args.gripper, args.tele) 296 | c.setup_and_run() 297 | 298 | rospy.spin() 299 | -------------------------------------------------------------------------------- /fetch_bringup/scripts/test/test_software_runstop.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # Copyright (c) 2016, Fetch Robotics Inc. 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | # 9 | # * Redistributions of source code must retain the above copyright 10 | # notice, this list of conditions and the following disclaimer. 11 | # * Redistributions in binary form must reproduce the above copyright 12 | # notice, this list of conditions and the following disclaimer in the 13 | # documentation and/or other materials provided with the distribution. 14 | # * Neither the name of the Fetch Robotics Inc. nor the names of its 15 | # contributors may be used to endorse or promote products derived from 16 | # this software without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | # ARE DISCLAIMED. IN NO EVENT SHALL FETCH ROBOTICS INC. BE LIABLE FOR ANY 22 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 27 | # THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | 29 | # Author: Eric Relson 30 | 31 | # Primarily unit tests logic of the update function. Run tests with nosetests 32 | # from the scripts/ directory. 33 | 34 | # Standard Library 35 | import unittest 36 | 37 | # Fetch 38 | from software_runstop import SoftwareRunstop 39 | 40 | 41 | def stub_set_breaker(*args): 42 | # For current tests, assume breaker was set successfully 43 | return True 44 | # Use stub to replace set_breaker() 45 | SoftwareRunstop.set_breaker = stub_set_breaker 46 | 47 | def stub_check_robot_components(self): 48 | # Current tests of init don't test robot components configuration 49 | self._has_arm = True 50 | self._has_base = True 51 | # Use stub to replace _check_robot_components() 52 | SoftwareRunstop._check_robot_components = stub_check_robot_components 53 | 54 | 55 | class TestInit(unittest.TestCase): 56 | 57 | def test_no_breakers(self): 58 | self.assertRaises(ValueError, SoftwareRunstop, 59 | False, False, False, True) 60 | 61 | def test_arg_arm(self): 62 | c = SoftwareRunstop(True, False, False, False) 63 | self.assertEqual(c.desired_breaker_states, 64 | {"/arm_breaker": True}) 65 | 66 | def test_arg_base(self): 67 | c = SoftwareRunstop(False, True, False, False) 68 | self.assertEqual(c.desired_breaker_states, 69 | {"/base_breaker": True}) 70 | 71 | def test_arg_gripper(self): 72 | c = SoftwareRunstop(False, False, True, False) 73 | self.assertEqual(c.desired_breaker_states, 74 | {"/gripper_breaker": True}) 75 | 76 | 77 | class TestUpdate(unittest.TestCase): 78 | 79 | def setUp(self): 80 | self.c = SoftwareRunstop(True, True, True, True) 81 | 82 | def assert_default_state(self): 83 | self.assertFalse(self.c.hw_runstop_has_been_pressed) 84 | self.assertFalse(self.c.hw_runstop_state) 85 | self.assertFalse(self.c.sw_runstop_enabled) 86 | self.assertFalse(self.c.enable_breakers) 87 | self.assertFalse(self.c.disable_breakers) 88 | self.assertFalse(self.c.disable_breakers_tele) 89 | self.assertFalse(self.c.disable_breakers_ext) 90 | 91 | def test_all_params_false(self): 92 | # Make sure params used in TestUpdate are initially False as expected 93 | self.assert_default_state() 94 | 95 | def test_tele_pressed_released_pressed(self): 96 | # Receive disable request, process and 97 | self.c.disable_breakers_tele = True 98 | self.c.update() 99 | self.assertFalse(self.c.disable_breakers) 100 | self.assertTrue(self.c.sw_runstop_enabled) 101 | # Next pass: no new disable signal from tele, expect no change 102 | self.c.update() 103 | self.assertFalse(self.c.disable_breakers) 104 | self.assertTrue(self.c.sw_runstop_enabled) 105 | # Next pass: button pressed again, but still no change 106 | self.c.disable_breakers_tele = True 107 | self.c.update() 108 | self.assertFalse(self.c.disable_breakers) 109 | self.assertTrue(self.c.sw_runstop_enabled) 110 | 111 | def test_disable_breaker_topic_received(self): 112 | # Receive disable request, process and 113 | self.c.disable_breakers_ext = True 114 | self.c.update() 115 | self.assertFalse(self.c.disable_breakers) 116 | self.assertTrue(self.c.sw_runstop_enabled) 117 | # Next pass: no new disable signal from tele, expect no change 118 | self.c.update() 119 | self.assertFalse(self.c.disable_breakers) 120 | self.assertTrue(self.c.sw_runstop_enabled) 121 | # Next pass: button pressed again, but still no change 122 | self.c.disable_breakers_tele = True 123 | self.c.update() 124 | self.assertFalse(self.c.disable_breakers) 125 | self.assertTrue(self.c.sw_runstop_enabled) 126 | 127 | def test_sw_runstop_released_by_hw_runstop(self): 128 | self.c.sw_runstop_enabled = True 129 | # Hardware runstop gets pressed 130 | self.c.hw_runstop_state = True 131 | self.c.update() 132 | self.assertTrue(self.c.hw_runstop_has_been_pressed) 133 | self.assertTrue(self.c.hw_runstop_state) 134 | self.assertTrue(self.c.sw_runstop_enabled) 135 | # and then released. Takes two updates before breakers re-enabled 136 | self.c.hw_runstop_state = False 137 | self.c.update() 138 | self.assertFalse(self.c.hw_runstop_has_been_pressed) 139 | self.assertFalse(self.c.hw_runstop_state) 140 | self.assertTrue(self.c.sw_runstop_enabled) 141 | self.assertTrue(self.c.enable_breakers) 142 | # The second update will enable breakers 143 | self.c.update() 144 | self.assert_default_state() 145 | 146 | def test_default_state_hw_runstop_no_effect(self): 147 | self.c.hw_runstop_state = True 148 | self.c.update() 149 | self.assertTrue(self.c.hw_runstop_state) 150 | self.assertFalse(self.c.hw_runstop_has_been_pressed) 151 | self.assertFalse(self.c.sw_runstop_enabled) 152 | self.assertFalse(self.c.enable_breakers) 153 | self.assertFalse(self.c.disable_breakers) 154 | 155 | def test_activate_sw_runstop_when_hw_runstopped(self): 156 | # We allow the sw runstop to be enabled while hardware runstop 157 | # This allows user to holde sw runstop button(s) on controller 158 | # while releasing the hw runstop, and then enable the breakers 159 | # after e.g. moving away from robot. 160 | self.c.hw_runstop_state = True 161 | self.c.disable_breakers = True 162 | self.c.update() 163 | self.assertTrue(self.c.hw_runstop_state) 164 | self.assertTrue(self.c.sw_runstop_enabled) 165 | self.assertFalse(self.c.hw_runstop_has_been_pressed) 166 | self.assertFalse(self.c.disable_breakers) 167 | # next update will count the hw runstop as having been pressed 168 | self.c.update() 169 | self.assertTrue(self.c.hw_runstop_state) 170 | self.assertTrue(self.c.hw_runstop_has_been_pressed) 171 | self.assertTrue(self.c.sw_runstop_enabled) 172 | 173 | def test_enable_breakers_works(self): 174 | # This test is currently redundant 175 | self.c.sw_runstop_enabled = True 176 | self.c.enable_breakers = True # set e.g. by enable_topic_callback 177 | # Go back to default state after software runstop is no longer active 178 | self.c.update() 179 | self.assert_default_state() 180 | 181 | -------------------------------------------------------------------------------- /freight_bringup/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package freight_bringup 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.7.2 (2016-06-14) 6 | ------------------ 7 | * add launch_camera argument 8 | * Contributors: Michael Ferguson 9 | 10 | 0.7.1 (2016-05-05) 11 | ------------------ 12 | * update to use /dev/ps3joy 13 | * Contributors: Michael Ferguson 14 | 15 | 0.7.0 (2016-03-28) 16 | ------------------ 17 | * fix dependency issue with sixad 18 | * Added PMD to freight.launch 19 | * Contributors: Alex Henning, Michael Ferguson 20 | 21 | 0.6.1 (2016-03-22) 22 | ------------------ 23 | * require latest sixad 24 | * Raised acceleration limits to 2.0 25 | * Add autorepeat_rate parameter to teleop launch 26 | * Contributors: Alex Henning, Michael Ferguson, Michael Hwang 27 | 28 | 0.6.0 (2015-06-23) 29 | ------------------ 30 | 31 | 0.5.5 (2015-05-21) 32 | ------------------ 33 | * use no_delay parameter with graft 34 | * Contributors: Michael Ferguson 35 | 36 | 0.5.4 (2015-05-10) 37 | ------------------ 38 | * filter shadow points from laser 39 | * reorganize launch files for easier updating of calibrated robots 40 | * Contributors: Michael Ferguson 41 | 42 | 0.5.3 (2015-05-03) 43 | ------------------ 44 | * use new laser safety feature of base controller 45 | * Contributors: Michael Ferguson 46 | 47 | 0.5.2 (2015-04-19) 48 | ------------------ 49 | * hold position when stopped 50 | * Contributors: Michael Ferguson 51 | 52 | 0.5.1 (2015-04-09) 53 | ------------------ 54 | 55 | 0.5.0 (2015-04-04) 56 | ------------------ 57 | 58 | 0.4.2 (2015-03-23) 59 | ------------------ 60 | * add depend on joy 61 | * Contributors: Michael Ferguson 62 | 63 | 0.4.1 (2015-03-23) 64 | ------------------ 65 | 66 | 0.4.0 (2015-03-22) 67 | ------------------ 68 | * update to use fetch_teleop 69 | * Contributors: Michael Ferguson 70 | 71 | 0.3.2 (2015-03-21) 72 | ------------------ 73 | * specify minimum version of laser drivers 74 | * update laser parameters 75 | * Contributors: Michael Ferguson 76 | 77 | 0.3.1 (2015-03-13 19:53) 78 | ------------------------ 79 | 80 | 0.3.0 (2015-03-13 18:59) 81 | ------------------------ 82 | * initial release 83 | * Contributors: Michael Ferguson 84 | -------------------------------------------------------------------------------- /freight_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(freight_bringup) 3 | 4 | find_package(catkin) 5 | catkin_package() 6 | 7 | install(DIRECTORY config 8 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 9 | ) 10 | 11 | install(DIRECTORY launch 12 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 13 | ) -------------------------------------------------------------------------------- /freight_bringup/config/default_controllers.yaml: -------------------------------------------------------------------------------- 1 | base_controller: 2 | type: "robot_controllers/DiffDriveBaseController" 3 | max_velocity_x: 1.5 4 | max_acceleration_x: 2.0 5 | # hold position 6 | moving_threshold: -0.01 7 | rotating_threshold: -0.01 8 | # autostart to get odom 9 | autostart: true 10 | # use laser to only slowly collide with things 11 | laser_safety_dist: 1.5 12 | 13 | robot_driver: 14 | default_controllers: 15 | - "base_controller" 16 | -------------------------------------------------------------------------------- /freight_bringup/config/graft.yaml: -------------------------------------------------------------------------------- 1 | planar_output: True # Output only x, y, and rotation about z 2 | 3 | output_frame: odom # TF frame id, param name ported from robot_pose_ekf 4 | parent_frame_id: odom # TF frame id, override output_frame if set 5 | child_frame_id: base_link #TF frame id 6 | publish_tf: true 7 | 8 | update_rate: 50.0 9 | update_topic: odom 10 | 11 | queue_size: 1 12 | 13 | # Filter parameters 14 | alpha: 0.001 15 | kappa: 0.0 16 | beta: 2.0 17 | 18 | # Process noise covariance 2x2 for velocites, 5x5 for 2d position 19 | process_noise: [1e6, 0, 0, 20 | 0, 1e6, 0, 21 | 0, 0, 1e6] 22 | 23 | # Inputs 24 | topics: { 25 | odom: { 26 | topic: /odom, 27 | type: nav_msgs/Odometry, 28 | absolute_pose: False, 29 | use_velocities: True, 30 | timeout: 1.0, 31 | no_delay: True, 32 | override_twist_covariance: [1e-3, 0, 0, 0, 0, 0, 33 | 0, 1e-3, 0, 0, 0, 0, 34 | 0, 0, 1e-6, 0, 0, 0, 35 | 0, 0, 0, 1e-6, 0, 0, 36 | 0, 0, 0, 0, 1e-6, 0, 37 | 0, 0, 0, 0, 0, 1e-3], 38 | }, 39 | 40 | imu: { 41 | topic: /imu, 42 | type: sensor_msgs/Imu, 43 | absolute_orientation: False, 44 | use_velocities: True, 45 | use_accelerations: False, 46 | timeout: 1.0, 47 | no_delay: True, 48 | override_angular_velocity_covariance: [0, 0, 0, 49 | 0, 0, 0, 50 | 0, 0, 0.000004], 51 | }, 52 | } 53 | -------------------------------------------------------------------------------- /freight_bringup/launch/freight.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 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /freight_bringup/launch/freight_r1.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 | -------------------------------------------------------------------------------- /freight_bringup/launch/include/base_camera_pmd.launch.xml: -------------------------------------------------------------------------------- 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 | 39 | 40 | 41 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /freight_bringup/launch/include/graft.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /freight_bringup/launch/include/laser.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /freight_bringup/launch/include/teleop.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /freight_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | freight_bringup 3 | 0.7.2 4 | 5 | Bringup for freight 6 | 7 | Michael Ferguson 8 | Michael Ferguson 9 | Proprietary 10 | catkin 11 | fetch_description 12 | fetch_drivers 13 | fetch_teleop 14 | graft 15 | joy 16 | pico_flexx_driver 17 | robot_state_publisher 18 | sick_tim 19 | sixad 20 | 21 | --------------------------------------------------------------------------------