diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/arm_joint_and_link_definitions.jpg b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/arm_joint_and_link_definitions.jpg deleted file mode 100644 index 5d97c90..0000000 Binary files a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/arm_joint_and_link_definitions.jpg and /dev/null differ diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_1.STL b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_1.STL deleted file mode 100644 index 1cc6dee..0000000 Binary files a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_1.STL and /dev/null differ diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_2.STL b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_2.STL deleted file mode 100644 index 7960a24..0000000 Binary files a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_2.STL and /dev/null differ diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_3.STL b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_3.STL deleted file mode 100644 index d81bd58..0000000 Binary files a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_3.STL and /dev/null differ diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_4.STL b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_4.STL deleted file mode 100644 index f764555..0000000 Binary files a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_4.STL and /dev/null differ diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_5.STL b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_5.STL deleted file mode 100644 index 1c02f3f..0000000 Binary files a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_5.STL and /dev/null differ diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_6.STL b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_6.STL deleted file mode 100644 index 3e2ee1c..0000000 Binary files a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_6.STL and /dev/null differ diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/base_link.STL b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/base_link.STL deleted file mode 100644 index 6469280..0000000 Binary files a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/meshes/base_link.STL and /dev/null differ diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.config b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.config deleted file mode 100644 index 05b32ed..0000000 --- a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.config +++ /dev/null @@ -1,11 +0,0 @@ - - - Fanuc_robot_arm - 1.0 - model.sdf - - - - - This was created using parts from a CAD model on GrabCAD - diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.sdf b/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.sdf deleted file mode 100644 index 3bbaf31..0000000 --- a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm/model.sdf +++ /dev/null @@ -1,905 +0,0 @@ - - - - - 0 0 0 3.14159 -0 0 - - 86.082 - - 4.3404 - 0 - 0.0020207 - 2.4866 - 0 - 6.1574 - - -0.003895 -0.062844 -0.11068 0 -0 0 - - 1 - 0 - 1 - - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/base_link.STL - 1 1 1 - - - - 1 - - - __default__ - - 0.3 0.3 0.3 1 - 0.7 0.7 0.7 1 - 0.01 0.01 0.01 1 - 0 0 0 1 - - 0 - 1 - - - 0 - 10 - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/base_link.STL - 1 1 1 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - - 0.001715 0.003779 0.661683 1.5708 -0 0 - - 266.27 - - 279.51 - 0 - 4.6477 - 43.386 - 34.265 - 255.26 - - 0.093634 -0.51398 -0.1614 0 -0 0 - - 1 - 0 - 1 - - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_1.STL - 1 1 1 - - - - 1 - - - __default__ - - 0.3 0.3 0.3 1 - 0.7 0.7 0.7 1 - 0.01 0.01 0.01 1 - 0 0 0 1 - - 0 - 1 - - - 0 - 10 - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_1.STL - 1 1 1 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - - -0.00025 -0.94659 5.45119 1.5708 -0 0 - - 63.612 - - 6.6124 - 0.099584 - 0 - 5.9295 - 0 - 1.9517 - - 0.00756 -3.4862 -0.40714 0 -0 0 - - 1 - 0 - 1 - - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_3.STL - 1 1 1 - - - - 1 - - - __default__ - - 0.3 0.3 0.3 1 - 0.7 0.7 0.7 1 - 0.01 0.01 0.01 1 - 0 0 0 1 - - 0 - 1 - - - 0 - 10 - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_3.STL - 1 1 1 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - - -0.068455 0.275759 0.315754 -1.571 -0 0 - - 60.795 - - 8.1939 - 0 - 0 - 0.76659 - 0 - 8.0226 - - 0.25344 -1.009 -0.57114 0 -0 0 - - 1 - 0 - 1 - - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_2.STL - 1 1 1 - - - - 1 - - - __default__ - - 0.3 0.3 0.3 1 - 0.7 0.7 0.7 1 - 0.01 0.01 0.01 1 - 0 0 0 1 - - 0 - 1 - - - 0 - 10 - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_2.STL - 1 1 1 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - - -0.000257 -3.97477 2.07064 -1.5708 7e-06 3.14159 - - 6.6264 - - 0.089718 - 0 - 0.017666 - 0.092881 - 3e-08 - 0.028564 - - -0.028814 0 -2.454 0 -0 0 - - 1 - 0 - 1 - - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_4.STL - 1 1 1 - - - - 1 - - - __default__ - - 0.3 0.3 0.3 1 - 0.7 0.7 0.7 1 - 0.01 0.01 0.01 1 - 0 0 0 1 - - 0 - 1 - - - 0 - 10 - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_4.STL - 1 1 1 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - - 0.000743 -4.767 2.06498 -1.5708 7e-06 3.14159 - - 5.1229 - - 0.038195 - 4e-07 - 0 - 0.066675 - 6e-07 - 0.04821 - - -0.010371 1e-06 -3.0669 0 -0 0 - - 1 - 0 - 1 - - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_5.STL - 1 1 1 - - - - 1 - - - __default__ - - 0.3 0.3 0.3 1 - 0.7 0.7 0.7 1 - 0.01 0.01 0.01 1 - 0 0 0 1 - - 0 - 1 - - - 0 - 10 - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_5.STL - 1 1 1 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - - -2.06426 -5.37015 4.13075 1.88426 1.57078 0.313465 - - 0.72893 - - 0.00131 - 0 - 0 - 0.0012978 - 0 - 0.0024341 - - 2.0657 2.0642 -3.5441 0 -0 0 - - 1 - 0 - 1 - - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_6.STL - 1 1 1 - - - - 1 - - - __default__ - - 0.3 0.3 0.3 1 - 0.7 0.7 0.7 1 - 0.01 0.01 0.01 1 - 0 0 0 1 - - 0 - 1 - - - 0 - 10 - 0 0 0 0 -0 0 - - - /home/rrrobot/rrrobot_src/src/gazebo_models/fanuc_robotic_arm/meshes/arm_link_6.STL - 1 1 1 - - - - - - 1 - 1 - 0 0 0 - 0 - 0 - - - 1 - 0 - 0 - 1 - - 0 - - - - - 0 - 1e+06 - - - 0 - 1 - 1 - - 0 - 0.2 - 1e+13 - 1 - 0.01 - 0 - - - 1 - -0.01 - 0 - 0.2 - 1e+13 - 1 - - - - - - - link_3 - link_4 - 0 0 -2.7 0 -0 0 - - 0 0 1 - 0 - - -1.79769e+308 - 1.79769e+308 - -2000 - -1 - - - 0 - 0 - 0 - 0 - - - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - link_1 - link_2 - 0 -0.41 -0.61 0 -0 0 - - 1 0 0 - 0 - - -1.79769e+308 - 1.79769e+308 - -2000 - -1 - - - 0 - 0 - 0 - 0 - - - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - link_2 - link_3 - 0 -3.65 -0.6 0 -0 0 - - 1 0 0 - 0 - - -1.79769e+308 - 1.79769e+308 - -2000 - -1 - - - 0 - 0 - 0 - 0 - - - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - link_4 - link_5 - 0 0 -3.15 0 -0 0 - - 1 0 0 - 0 - - -1.79769e+308 - 1.79769e+308 - -2000 - -1 - - - 0 - 0 - 0 - 0 - - - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - base - link_1 - 0 0 0 0 -0 0 - - 0 1 0 - 0 - - -1.79769e+308 - 1.79769e+308 - -2000 - -1 - - - 0 - 0 - 0 - 0 - - - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - link_5 - link_6 - 2.065 2.065 -3.5 0 -0 0 - - 0 0 1 - 0 - - -1.79769e+308 - 1.79769e+308 - -2000 - -1 - - - 0 - 0 - 0 - 0 - - - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - world - base - - 0 - 0 - - - - - diff --git a/rrrobot_ws/src/rrrobot/CMakeLists.txt b/rrrobot_ws/src/rrrobot/CMakeLists.txt new file mode 100644 index 0000000..35f03c2 --- /dev/null +++ b/rrrobot_ws/src/rrrobot/CMakeLists.txt @@ -0,0 +1,75 @@ +cmake_minimum_required(VERSION 2.8.3) +project(rrrobot) + +find_package(catkin REQUIRED COMPONENTS + osrf_gear + roscpp + sensor_msgs + std_srvs + trajectory_msgs +) + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +catkin_package() + +########### +## Build ## +########### + +include_directories(include ${catkin_INCLUDE_DIRS}) + +## Declare a C++ executable +add_executable(rrrobot_node src/rrrobot_node.cpp) +add_dependencies(rrrobot_node ${catkin_EXPORTED_TARGETS}) +target_link_libraries(rrrobot_node ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +install(PROGRAMS + script/rrrobot_node.py + script/tf2_example.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark executables and/or libraries for installation +install(TARGETS rrrobot_node + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +install(FILES + config/sample_gear_conf.yaml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config +) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_ariac_example.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) \ No newline at end of file diff --git a/rrrobot_ws/src/rrrobot/config/rrrobot.yaml b/rrrobot_ws/src/rrrobot/config/rrrobot.yaml new file mode 100644 index 0000000..242144e --- /dev/null +++ b/rrrobot_ws/src/rrrobot/config/rrrobot.yaml @@ -0,0 +1,163 @@ +# Competition configuration options +options: + insert_models_over_bins: true # Whether or not to insert the models that are specified in models_over_bins + spawn_extra_models: true # Whether or not to spawn the models that are specified in models_to_spawn + gazebo_state_logging: true # Whether or not to generate a gazebo state log + belt_population_cycles: 5 # How many cycles to spawn parts on the conveyor + model_type_aliases: # Aliases for model types which can be used in the configuration file + order_part1: piston_rod_part # Wherever 'order_part1' is used in the configuration file, use 'piston_rod_part' + order_part2: gear_part + order_part3: pulley_part + visualize_drop_regions: false # Whether or not to visualize drop regions (world frame only) + +time_limit: 500 # Maximum time allowed for the trial once started, in seconds +random_seed: 1 # Seed for the pseudo random number generator (used to randomize model names) + +# Orders to announce during the trial +# orders: +# order_0: +# announcement_condition: time # Announce the order base on elapsed time +# announcement_condition_value: 0 # Time in seconds to wait before announcing the order +# shipment_count: 1 # How many of the same shipment are in the order +# destinations: [agv1] # Which agv the shipments must be delivered to +# # If specified it must be a list the same length as the shipment count +# # allowed values: agv1, agv2, any +# products: # List of products required to be in the kit +# part_0: +# type: order_part1 # Type of model required +# pose: +# xyz: [0.1, -0.15, 0] # Position required in the tray frame +# rpy: [0, 0, 'pi/2'] # Orientation required in the tray frame +# part_1: +# type: order_part1 +# pose: +# xyz: [-0.1, -0.15, 0] +# rpy: [0, 0, 'pi/2'] +# part_2: +# type: order_part2 +# pose: +# xyz: [0.1, 0.15, 0] +# rpy: [0, 0, 0] +# part_3: +# type: order_part2 +# pose: +# xyz: [-0.1, 0.15, 0] +# rpy: [0, 0, 0] +# order_1: +# announcement_condition: wanted_products # Announce the order when the boxes contain products from this order +# announcement_condition_value: 2 +# shipment_count: 1 +# destinations: [any] +# products: +# part_0: +# type: order_part3 +# pose: +# xyz: [0.12, -0.2, 0] +# rpy: ['pi', 0, 0] +# part_1: +# type: order_part3 +# pose: +# xyz: [-0.12, -0.2, 0] +# rpy: [0, 'pi', 0] +# part_2: +# type: order_part1 +# pose: +# xyz: [0.15, 0.15, 0] +# rpy: [0, 0, 0] +# part_3: +# type: order_part2 +# pose: +# xyz: [-0.15, 0.15, 0] +# rpy: [0, 0, 0] +# part_4: +# type: order_part2 +# pose: +# rpy: [0, 'pi', 0] + +# # Individual products that will be reported as faulty +# faulty_products: +# - piston_rod_part_57 # The piston rod part in the bins with randomized ID of 57 +# - piston_rod_part_45 + +# Models to be inserted in the bins +models_over_bins: + bin1: # Name of the bin (bin1-bin6, as named in the environment simulation) + models: # List of models to insert + # gear_part: # Type of model to insert + # xyz_start: [0.1, 0.1, 0] # Origin of the first model to insert + # xyz_end: [0.5, 0.5, 0] # Origin of the last model to insert + # rpy: [0, 0, 'pi/4'] # Orientation of all models to insert + # num_models_x: 3 # How many models to insert along the x dimension + # num_models_y: 5 # How many models to insert along the y dimension + # bin5: + # models: + # gasket_part: + # xyz_start: [0.1, 0.1, 0.0] + # xyz_end: [0.5, 0.5, 0.0] + # rpy: [0, 0, 'pi/4'] + # num_models_x: 2 + # num_models_y: 3 + # bin6: + # models: + # piston_rod_part: + # xyz_start: [0.1, 0.1, 0.0] + # xyz_end: [0.5, 0.5, 0.0] + # rpy: [0, 0, 'pi/4'] + # num_models_x: 3 + # num_models_y: 3 + +# # Models to be spawned in particular reference frames +# models_to_spawn: +# bin2::link: # Name of the reference frame +# models: # List of models to spawn +# piston_rod_part_1: # An arbitrary unique name of the model (will be randomized) +# type: piston_rod_part # Type of model (must be installed locally) +# pose: +# xyz: [0.2, 0.2, 0.75] # Co-ordinates of the model origin in the specified reference frame +# rpy: [0, 0, '-pi/2'] # Roll, pitch, yaw of the model in the specified reference frame + +# # Models to be spawned on the conveyor belt +# belt_models: +# pulley_part: # name of part model to spawn +# 1.0: # Time in seconds after trial starts to spawn it +# pose: +# xyz: [0.0, 0.0, 0.1] # Coordinates relative to the start of the container to spawn the part +# rpy: [0, 0, 'pi/2'] # Roll, pitch, yaw of the model +# disk_part: +# 5.0: +# pose: +# xyz: [0.0, 0.0, 0.1] +# rpy: [0, 0, 'pi/2'] + +# # Drops from the vacuum gripper to be triggered at particular locations +# drops: +# drop_regions: +# above_agv_1: +# frame: agv1::kit_tray_1 # Frame the drop region/destination is specified in +# min: # Min corner of the bounding box that triggers a drop +# xyz: [-0.3, -0.3, 0.0] +# max: # Max corner of the bounding box that triggers a drop +# xyz: [0.3, 03, 0.5] +# destination: # Where to drop the part to +# xyz: [-0.35, 0.1, 0.15] +# rpy: [0, 0, 0.2] +# product_type_to_drop: gear_part +# above_agv_2: +# frame: agv2::kit_tray_2 +# min: +# xyz: [-0.3, -0.3, 0.0] +# max: +# xyz: [0.3, 0.3, 0.5] +# destination: +# xyz: [0.15, 0.15, 0.15] +# rpy: [0, 0, -0.2] +# product_type_to_drop: pulley_part +# bin5_reachable: +# min: +# xyz: [0.0, 0.7, 0.7] +# max: +# xyz: [0.2, 1.60, 1.3] +# destination: +# xyz: [0.65, 1.15, 0.76] +# rpy: [0, 0, 0.5] +# product_type_to_drop: gasket_part diff --git a/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml b/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml new file mode 100644 index 0000000..33de276 --- /dev/null +++ b/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml @@ -0,0 +1,36 @@ +sensors: + break_beam_1: + type: break_beam + pose: + xyz: [1.6, 2.25, 0.91] + rpy: [0, 0, 'pi'] + proximity_sensor_1: + type: proximity_sensor + pose: + xyz: [0.95, 2.6, 0.92] + rpy: [0, 0, 0] + logical_camera_1: + type: logical_camera + pose: + xyz: [-0.3, 0.383, 1.27] + rpy: [0.2, 1.5707, 0] + logical_camera_2: + type: logical_camera + pose: + xyz: [0.3, 3.15, 1.5] + rpy: [0, 'pi/2', 0] + logical_camera_3: + type: logical_camera + pose: + xyz: [0.3, -3.15, 1.5] + rpy: [0, 'pi/2', 0] + depth_camera_1: + type: depth_camera + pose: + xyz: [-0.3, -0.383, 1.2] + rpy: [0, 1.5707, 0] + laser_profiler_1: + type: laser_profiler + pose: + xyz: [1.21, 4, 1.6] + rpy: ['-pi', 'pi/2', 'pi/2'] diff --git a/rrrobot_ws/src/rrrobot/launch/rrrobot.launch b/rrrobot_ws/src/rrrobot/launch/rrrobot.launch new file mode 100644 index 0000000..224c26d --- /dev/null +++ b/rrrobot_ws/src/rrrobot/launch/rrrobot.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + diff --git a/rrrobot_ws/src/rrrobot/package.xml b/rrrobot_ws/src/rrrobot/package.xml new file mode 100644 index 0000000..eb940a9 --- /dev/null +++ b/rrrobot_ws/src/rrrobot/package.xml @@ -0,0 +1,22 @@ + + + rrrobot + 0.1.0 + RRRobot package + Sravan Balaji + Chenxi Gu + Jake Johnson + Derek Witcpalek + + MIT + + catkin + + osrf_gear + roscpp + sensor_msgs + std_srvs + tf2_geometry_msgs + trajectory_msgs + + \ No newline at end of file diff --git a/rrrobot_ws/src/rrrobot/scripts/rrrobot_run.sh b/rrrobot_ws/src/rrrobot/scripts/rrrobot_run.sh new file mode 100644 index 0000000..41155ec --- /dev/null +++ b/rrrobot_ws/src/rrrobot/scripts/rrrobot_run.sh @@ -0,0 +1,5 @@ +#!/bin/bash + +sudo cp ../launch/rrrobot.launch /opt/ros/melodic/share/osrf_gear/launch + +roslaunch osrf_gear rrrobot.launch diff --git a/rrrobot_ws/src/gear/sample_run.sh b/rrrobot_ws/src/rrrobot/scripts/sample_run.sh similarity index 100% rename from rrrobot_ws/src/gear/sample_run.sh rename to rrrobot_ws/src/rrrobot/scripts/sample_run.sh diff --git a/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp b/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp new file mode 100644 index 0000000..2314b17 --- /dev/null +++ b/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp @@ -0,0 +1,261 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// %Tag(FULLTEXT)% +// %Tag(INCLUDE_STATEMENTS)% +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +// %EndTag(INCLUDE_STATEMENTS)% + +// %Tag(START_COMP)% +/// Start the competition by waiting for and then calling the start ROS Service. +void start_competition(ros::NodeHandle & node) { + // Create a Service client for the correct service, i.e. '/ariac/start_competition'. + ros::ServiceClient start_client = + node.serviceClient("/ariac/start_competition"); + // If it's not already ready, wait for it to be ready. + // Calling the Service using the client before the server is ready would fail. + if (!start_client.exists()) { + ROS_INFO("Waiting for the competition to be ready..."); + start_client.waitForExistence(); + ROS_INFO("Competition is now ready."); + } + ROS_INFO("Requesting competition start..."); + std_srvs::Trigger srv; // Combination of the "request" and the "response". + start_client.call(srv); // Call the start Service. + if (!srv.response.success) { // If not successful, print out why. + ROS_ERROR_STREAM("Failed to start the competition: " << srv.response.message); + } else { + ROS_INFO("Competition started!"); + } +} +// %EndTag(START_COMP)% + +/// Example class that can hold state and provide methods that handle incoming data. +class MyCompetitionClass +{ +public: + explicit MyCompetitionClass(ros::NodeHandle & node) + : current_score_(0), arm_1_has_been_zeroed_(false), arm_2_has_been_zeroed_(false) + { + // %Tag(ADV_CMD)% + arm_1_joint_trajectory_publisher_ = node.advertise( + "/ariac/arm1/arm/command", 10); + + arm_2_joint_trajectory_publisher_ = node.advertise( + "/ariac/arm2/arm/command", 10); + // %EndTag(ADV_CMD)% + } + + /// Called when a new message is received. + void current_score_callback(const std_msgs::Float32::ConstPtr & msg) { + if (msg->data != current_score_) + { + ROS_INFO_STREAM("Score: " << msg->data); + } + current_score_ = msg->data; + } + + /// Called when a new message is received. + void competition_state_callback(const std_msgs::String::ConstPtr & msg) { + if (msg->data == "done" && competition_state_ != "done") + { + ROS_INFO("Competition ended."); + } + competition_state_ = msg->data; + } + + /// Called when a new Order message is received. + void order_callback(const osrf_gear::Order::ConstPtr & order_msg) { + ROS_INFO_STREAM("Received order:\n" << *order_msg); + received_orders_.push_back(*order_msg); + } + + // %Tag(CB_CLASS)% + /// Called when a new JointState message is received. + void arm_1_joint_state_callback( + const sensor_msgs::JointState::ConstPtr & joint_state_msg) + { + ROS_INFO_STREAM_THROTTLE(10, + "Joint States arm 1 (throttled to 0.1 Hz):\n" << *joint_state_msg); + // ROS_INFO_STREAM("Joint States:\n" << *joint_state_msg); + arm_1_current_joint_states_ = *joint_state_msg; + if (!arm_1_has_been_zeroed_) { + arm_1_has_been_zeroed_ = true; + ROS_INFO("Sending arm to zero joint positions..."); + send_arm_to_zero_state(arm_1_joint_trajectory_publisher_); + } + } + + void arm_2_joint_state_callback( + const sensor_msgs::JointState::ConstPtr & joint_state_msg) + { + ROS_INFO_STREAM_THROTTLE(10, + "Joint States arm 2 (throttled to 0.1 Hz):\n" << *joint_state_msg); + // ROS_INFO_STREAM("Joint States:\n" << *joint_state_msg); + arm_2_current_joint_states_ = *joint_state_msg; + if (!arm_2_has_been_zeroed_) { + arm_2_has_been_zeroed_ = true; + ROS_INFO("Sending arm 2 to zero joint positions..."); + send_arm_to_zero_state(arm_2_joint_trajectory_publisher_); + } + } + // %EndTag(CB_CLASS)% + + // %Tag(ARM_ZERO)% + /// Create a JointTrajectory with all positions set to zero, and command the arm. + void send_arm_to_zero_state(ros::Publisher & joint_trajectory_publisher) { + // Create a message to send. + trajectory_msgs::JointTrajectory msg; + + // Fill the names of the joints to be controlled. + // Note that the vacuum_gripper_joint is not controllable. + msg.joint_names.clear(); + msg.joint_names.push_back("shoulder_pan_joint"); + msg.joint_names.push_back("shoulder_lift_joint"); + msg.joint_names.push_back("elbow_joint"); + msg.joint_names.push_back("wrist_1_joint"); + msg.joint_names.push_back("wrist_2_joint"); + msg.joint_names.push_back("wrist_3_joint"); + msg.joint_names.push_back("linear_arm_actuator_joint"); + // Create one point in the trajectory. + msg.points.resize(1); + // Resize the vector to the same length as the joint names. + // Values are initialized to 0. + msg.points[0].positions.resize(msg.joint_names.size(), 0.0); + // How long to take getting to the point (floating point seconds). + msg.points[0].time_from_start = ros::Duration(0.001); + ROS_INFO_STREAM("Sending command:\n" << msg); + joint_trajectory_publisher.publish(msg); + } + // %EndTag(ARM_ZERO)% + + /// Called when a new LogicalCameraImage message is received. + void logical_camera_callback( + const osrf_gear::LogicalCameraImage::ConstPtr & image_msg) + { + ROS_INFO_STREAM_THROTTLE(10, + "Logical camera: '" << image_msg->models.size() << "' objects."); + } + + /// Called when a new Proximity message is received. + void break_beam_callback(const osrf_gear::Proximity::ConstPtr & msg) { + if (msg->object_detected) { // If there is an object in proximity. + ROS_INFO("Break beam triggered."); + } + } + +private: + std::string competition_state_; + double current_score_; + ros::Publisher arm_1_joint_trajectory_publisher_; + ros::Publisher arm_2_joint_trajectory_publisher_; + std::vector received_orders_; + sensor_msgs::JointState arm_1_current_joint_states_; + sensor_msgs::JointState arm_2_current_joint_states_; + bool arm_1_has_been_zeroed_; + bool arm_2_has_been_zeroed_; +}; + +void proximity_sensor_callback(const sensor_msgs::Range::ConstPtr & msg) { + if ((msg->max_range - msg->range) > 0.01) { // If there is an object in proximity. + ROS_INFO_THROTTLE(1, "Proximity sensor sees something."); + } +} + +void laser_profiler_callback(const sensor_msgs::LaserScan::ConstPtr & msg) { + size_t number_of_valid_ranges = std::count_if( + msg->ranges.begin(), msg->ranges.end(), [](const float f) {return std::isfinite(f);}); + if (number_of_valid_ranges > 0) { + ROS_INFO_THROTTLE(1, "Laser profiler sees something."); + } +} + +// %Tag(MAIN)% +int main(int argc, char ** argv) { + // Last argument is the default name of the node. + ros::init(argc, argv, "rrrobot_node"); + + ros::NodeHandle node; + + // Instance of custom class from above. + MyCompetitionClass comp_class(node); + + // Subscribe to the '/ariac/current_score' topic. + ros::Subscriber current_score_subscriber = node.subscribe( + "/ariac/current_score", 10, + &MyCompetitionClass::current_score_callback, &comp_class); + + // Subscribe to the '/ariac/competition_state' topic. + ros::Subscriber competition_state_subscriber = node.subscribe( + "/ariac/competition_state", 10, + &MyCompetitionClass::competition_state_callback, &comp_class); + + // %Tag(SUB_CLASS)% + // Subscribe to the '/ariac/orders' topic. + ros::Subscriber orders_subscriber = node.subscribe( + "/ariac/orders", 10, + &MyCompetitionClass::order_callback, &comp_class); + // %EndTag(SUB_CLASS)% + + // Subscribe to the '/ariac/joint_states' topic. + ros::Subscriber arm_1_joint_state_subscriber = node.subscribe( + "/ariac/arm1/joint_states", 10, + &MyCompetitionClass::arm_1_joint_state_callback, &comp_class); + + ros::Subscriber arm_2_joint_state_subscriber = node.subscribe( + "/ariac/arm2/joint_states", 10, + &MyCompetitionClass::arm_2_joint_state_callback, &comp_class); + + // %Tag(SUB_FUNC)% + // Subscribe to the '/ariac/proximity_sensor_1' topic. + ros::Subscriber proximity_sensor_subscriber = node.subscribe( + "/ariac/proximity_sensor_1", 10, proximity_sensor_callback); + // %EndTag(SUB_FUNC)% + + // Subscribe to the '/ariac/break_beam_1_change' topic. + ros::Subscriber break_beam_subscriber = node.subscribe( + "/ariac/break_beam_1_change", 10, + &MyCompetitionClass::break_beam_callback, &comp_class); + + // Subscribe to the '/ariac/logical_camera_1' topic. + ros::Subscriber logical_camera_subscriber = node.subscribe( + "/ariac/logical_camera_1", 10, + &MyCompetitionClass::logical_camera_callback, &comp_class); + + // Subscribe to the '/ariac/laser_profiler_1' topic. + ros::Subscriber laser_profiler_subscriber = node.subscribe( + "/ariac/laser_profiler_1", 10, laser_profiler_callback); + + ROS_INFO("Setup complete."); + start_competition(node); + ros::spin(); // This executes callbacks on new data until ctrl-c. + + return 0; +} +// %EndTag(MAIN)% +// %EndTag(FULLTEXT)% diff --git a/rrrobot_ws/src/simulation_env/CMakeLists.txt b/rrrobot_ws/src/simulation_env/CMakeLists.txt deleted file mode 100644 index 5be384d..0000000 --- a/rrrobot_ws/src/simulation_env/CMakeLists.txt +++ /dev/null @@ -1,225 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(simulation_env) - -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - gazebo_ros - message_generation -) -find_package(gazebo REQUIRED) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -add_message_files( - FILES - arm_angles.msg - arm_command.msg -) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs -) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include -# LIBRARIES simulation_env - CATKIN_DEPENDS roscpp std_msgs gazebo_ros -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${roscpp_INCLUDE_DIRS} - ${std_msgs_INCLUDE_DIRS} - ${GAZEBO_INCLUDE_DIRS} -) - -## Declare a C++ library -add_library(arm_angles SHARED - src/arm_angle_sensor_plugin.cpp -) - -add_library(arm_motors SHARED - src/arm_motors.cpp -) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -add_executable(test_move_arm test/move_arm.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -add_dependencies(test_move_arm arm_angles arm_motors) - -## Specify libraries to link a library or executable target against -target_link_libraries(arm_angles - ${catkin_LIBRARIES} - ${GAZEBO_LIBRARIES} -) - -target_link_libraries(arm_motors - ${catkin_LIBRARIES} - ${GAZEBO_LIBRARIES} -) - -target_link_libraries(test_move_arm - ${catkin_LIBRARIES} -) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -install(TARGETS arm_angles -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION /home/rrrobot/rrrobot_src/lib/ -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_simulation_env.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/rrrobot_ws/src/simulation_env/msg/arm_angles.msg b/rrrobot_ws/src/simulation_env/msg/arm_angles.msg deleted file mode 100644 index a6e84c6..0000000 --- a/rrrobot_ws/src/simulation_env/msg/arm_angles.msg +++ /dev/null @@ -1,6 +0,0 @@ -float32 shoulder_pivot_angle -float32 shoulder_joint_angle -float32 elbow_joint_angle -float32 wrist_pivot_angle -float32 wrist_joint_angle -float32 end_effector_pivot_angle diff --git a/rrrobot_ws/src/simulation_env/msg/arm_command.msg b/rrrobot_ws/src/simulation_env/msg/arm_command.msg deleted file mode 100644 index 419c9ca..0000000 --- a/rrrobot_ws/src/simulation_env/msg/arm_command.msg +++ /dev/null @@ -1,6 +0,0 @@ -float32 shoulder_pivot_force -float32 shoulder_joint_force -float32 elbow_joint_force -float32 wrist_pivot_force -float32 wrist_joint_force -float32 end_effector_pivot_force diff --git a/rrrobot_ws/src/simulation_env/package.xml b/rrrobot_ws/src/simulation_env/package.xml deleted file mode 100644 index 6efb6ec..0000000 --- a/rrrobot_ws/src/simulation_env/package.xml +++ /dev/null @@ -1,70 +0,0 @@ - - - simulation_env - 0.0.0 - The simulation_env package - - - - - root - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - std_msgs - gazebo_ros - message_generation - roscpp - std_msgs - gazebo_ros - roscpp - std_msgs - gazebo_ros - message_runtime - - - - - - - - diff --git a/rrrobot_ws/src/simulation_env/src/arm_angle_sensor_plugin.cpp b/rrrobot_ws/src/simulation_env/src/arm_angle_sensor_plugin.cpp deleted file mode 100644 index 806743b..0000000 --- a/rrrobot_ws/src/simulation_env/src/arm_angle_sensor_plugin.cpp +++ /dev/null @@ -1,61 +0,0 @@ -#include -#include -#include -#include - -#include "ros/ros.h" -#include "simulation_env/arm_angles.h" - -using std::cout; -using std::endl; - -namespace gazebo -{ -class JointAngle : public ModelPlugin -{ -public: - void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) - { - cout << "Loading JointAngle sensor plugin for arm" << endl; - - model = _parent; - - updateConnection = event::Events::ConnectWorldUpdateBegin(std::bind(&JointAngle::onUpdate, this)); - if (!ros::isInitialized()) - { - int argc = 0; - char **argv = NULL; - ros::init(argc, argv, "arm"); - } - - nh.reset(new ros::NodeHandle("arm_node")); - publisher = nh->advertise("arm_positions", 1); - } - -private: - void onUpdate() - { - simulation_env::arm_angles msg; - - // read in the joint angle for each joint in the arm - msg.shoulder_pivot_angle = model->GetJoint("shoulder_pivot")->Position(); - msg.shoulder_joint_angle = model->GetJoint("shoulder_joint")->Position(); - msg.elbow_joint_angle = model->GetJoint("elbow_joint")->Position(); - msg.wrist_pivot_angle = model->GetJoint("wrist_pivot")->Position(); - msg.wrist_joint_angle = model->GetJoint("wrist_joint")->Position(); - msg.end_effector_pivot_angle = model->GetJoint("end_effector_pivot")->Position(); - - // publish the updated sensor measurements - publisher.publish(msg); - - // make sure the message gets published - ros::spinOnce(); - } - - physics::ModelPtr model; - event::ConnectionPtr updateConnection; - std::unique_ptr nh; - ros::Publisher publisher; -}; -GZ_REGISTER_MODEL_PLUGIN(JointAngle) -} // namespace gazebo diff --git a/rrrobot_ws/src/simulation_env/src/arm_motors.cpp b/rrrobot_ws/src/simulation_env/src/arm_motors.cpp deleted file mode 100644 index 099f036..0000000 --- a/rrrobot_ws/src/simulation_env/src/arm_motors.cpp +++ /dev/null @@ -1,53 +0,0 @@ -#include -#include -#include -#include - -#include "ros/ros.h" -#include "simulation_env/arm_command.h" - -using std::cout; -using std::endl; - -namespace gazebo -{ -class ArmControl : public ModelPlugin -{ -public: - void arm_command_callback(const simulation_env::arm_command &cmd) - { - // update the torque applied to each joint when a message is received - model->GetJoint("shoulder_pivot")->SetForce(0, cmd.shoulder_pivot_force); - model->GetJoint("shoulder_joint")->SetForce(0, cmd.shoulder_joint_force); - model->GetJoint("elbow_joint")->SetForce(0, cmd.elbow_joint_force); - model->GetJoint("wrist_pivot")->SetForce(0, cmd.wrist_pivot_force); - model->GetJoint("wrist_joint")->SetForce(0, cmd.wrist_joint_force); - model->GetJoint("end_effector_pivot")->SetForce(0, cmd.end_effector_pivot_force); - - ros::spinOnce(); - } - - void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) - { - cout << "Loading motor control plugin for arm" << endl; - - model = _parent; - - if (!ros::isInitialized()) - { - int argc = 0; - char **argv = NULL; - ros::init(argc, argv, "arm"); - } - - nh.reset(new ros::NodeHandle("arm_node")); - subscriber = nh->subscribe("arm_commands", 1000, &ArmControl::arm_command_callback, this); - } - -private: - physics::ModelPtr model; - std::unique_ptr nh; - ros::Subscriber subscriber; -}; -GZ_REGISTER_MODEL_PLUGIN(ArmControl) -} // namespace gazebo diff --git a/rrrobot_ws/src/simulation_env/test/move_arm.cpp b/rrrobot_ws/src/simulation_env/test/move_arm.cpp deleted file mode 100644 index abd0c46..0000000 --- a/rrrobot_ws/src/simulation_env/test/move_arm.cpp +++ /dev/null @@ -1,70 +0,0 @@ -#include "ros/ros.h" -#include "simulation_env/arm_command.h" -#include "simulation_env/arm_angles.h" - -class PID_Control -{ -public: - PID_Control(float Kp_in, float Ki_in, float Kd_in) - : Kp(Kp_in), Ki(Ki_in), Kd(Kd_in) - {} - - float update(float error) - { - if(error > 0 and prev_error < 0 - || error < 0 and prev_error > 0) - { - reset(); - } - sum_error += error; - float to_ret = Kp * error + Ki * sum_error + Kd * (error - prev_error); - - prev_error = error; - return to_ret; - } - - void reset() - { - sum_error = 0; - prev_error = 0; - } - -private: - const float Kp; - const float Ki; - const float Kd; - - double sum_error; - double prev_error; -}; - - -PID_Control pid_controller(1000, 1, 0); -ros::Publisher publisher; - - - -void angle_callback(const simulation_env::arm_angles &msg) -{ - // just control the shoulder joint to go to 0 (should be roughly straight up) - float output = pid_controller.update(0 - msg.shoulder_joint_angle); - - simulation_env::arm_command cmd; - cmd.shoulder_joint_force = output; - - publisher.publish(cmd); - ros::spinOnce(); -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "arm_control_test"); - - ros::NodeHandle nh; - publisher = nh.advertise("/arm_node/arm_commands", 1000); - ros::Subscriber sub = nh.subscribe("/arm_node/arm_positions", 1000, angle_callback); - - ros::spin(); - - return 0; -} \ No newline at end of file diff --git a/rrrobot_ws/world/rrrobot.world b/rrrobot_ws/world/rrrobot.world deleted file mode 100644 index dddad2a..0000000 --- a/rrrobot_ws/world/rrrobot.world +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - model://ground_plane - - - - model://sun - - - - model://fanuc_robotic_arm - - -