Merge pull request #24 from cc0h/master

Merge engineer code

engineer_arm_config/config/engineer.srdf

@@ -16,6 +16,7 @@
         <joint name="joint4"/>
         <joint name="joint5"/>
         <joint name="joint6"/>
+        <joint name="joint7"/>
     </group>
     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
     <group_state name="home" group="engineer_arm">
@@ -25,6 +26,7 @@
         <joint name="joint4" value="0"/>
         <joint name="joint5" value="1.92225"/>
         <joint name="joint6" value="0"/>
+        <joint name="joint7" value="0"/>
     </group_state>
         <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
         <disable_collisions link1="base_link" link2="gimbal_base" reason="Adjacent"/>
@@ -42,11 +44,10 @@
         <disable_collisions link1="link2" link2="link4" reason="Never"/>
         <disable_collisions link1="link3" link2="link4" reason="Adjacent"/>
         <disable_collisions link1="link3" link2="link5" reason="Default"/>
-        <disable_collisions link1="link3" link2="link6" reason="Never"/>
         <disable_collisions link1="link4" link2="link5" reason="Adjacent"/>
         <disable_collisions link1="link4" link2="link6" reason="Never"/>
         <disable_collisions link1="link5" link2="link6" reason="Adjacent"/>
-        <disable_collisions link1="link3" link2="link7" reason="Never"/>
         <disable_collisions link1="link6" link2="link7" reason="Never"/>
         <disable_collisions link1="link5" link2="link7" reason="Never"/>
+        <disable_collisions link1="link1" link2="camera_link" reason="Never"/>
 </robot>

engineer_arm_config/config/fake_controllers.yaml

@@ -8,6 +8,7 @@ controller_list:
       - joint4
       - joint5
       - joint6
+      - joint7
 initial:  # Define initial robot poses per group
   - group: engineer_arm
     pose: home

engineer_arm_config/config/joint_limits.yaml

@@ -38,3 +38,8 @@ joint_limits:
     max_velocity: 50
     has_acceleration_limits: true
     max_acceleration: 90
+  joint7:
+    has_velocity_limits: true
+    max_velocity: 50
+    has_acceleration_limits: true
+    max_acceleration: 90

engineer_arm_config/config/kinematics.yaml

@@ -1,4 +1,4 @@
 engineer_arm:
-  kinematics_solver: engineer_arm/IKFastKinematicsPlugin
-  kinematics_solver_search_resolution: 0.03
-  kinematics_solver_timeout: 0.03
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.005

engineer_arm_config/config/ros_controllers.yaml

@@ -15,6 +15,7 @@ hardware_interface:
     - joint4
     - joint5
     - joint6
+    - joint7
   sim_control_mode: 1  # 0: position, 1: velocity
 
 controller_list:
@@ -29,3 +30,4 @@ controller_list:
       - joint4
       - joint5
       - joint6
+      - joint7

engineer_arm_config/config/servo.yaml

@@ -54,7 +54,7 @@ status_topic: status # Publish status to this topic
 command_out_topic: /controllers/arm_trajectory_controller/command # Publish outgoing commands here
 
 ## Collision checking for the entire robot body
-check_collisions: true # Check collisions?
+check_collisions: false # Check collisions?
 collision_check_rate: 50 # [Hz] Collision-checking can easily bog down a CPU if done too often.
 # Two collision check algorithms are available:
 # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually.

engineer_arm_config/launch/load_controllers.launch

@@ -2,12 +2,20 @@
 <launch>
     <!-- By default, we are not load controllers -->
     <arg name="spawn_controllers" default="true"/>
+    <arg name="use_simulation" default="false"/>
+
+    <group unless="$(arg use_simulation)">
+        <rosparam file="$(find rm_config)/config/rm_controllers/engineer.yaml" command="load"/>
+    </group>
+    <group if="$(arg use_simulation)">
+        <rosparam file="$(find engineer_middleware)/config/simulation_controllers.yaml" command="load"/>
+    </group>
 
-    <rosparam file="$(find rm_config)/config/rm_controllers/engineer.yaml" command="load"/>
 
     <!-- Load joint controller configurations from YAML file to parameter server -->
 
     <!-- Load the controllers -->
+
     <node name="controller_loader" pkg="controller_manager" type="controller_manager" output="screen"
           args="load
                 controllers/robot_state_controller
@@ -18,18 +26,20 @@
                 controllers/joint4_calibration_controller
                 controllers/joint5_calibration_controller
                 controllers/joint6_calibration_controller
+                controllers/joint7_calibration_controller
                 controllers/arm_trajectory_controller
                 controllers/chassis_controller
                 controllers/gpio_controller
-                controllers/pitch_front_controller
-                controllers/pitch_behind_controller
-                controllers/roll_left_controller
-                controllers/roll_right_controller
-                controllers/drag_controller
-                controllers/drag_calibration_controller
-                controllers/pitch_calibration_controller
-                controllers/yaw_calibration_controller
                 controllers/gimbal_controller
+                controllers/ore_bin_rotate_controller
+                controllers/ore_bin_lifter_controller
+                controllers/ore_bin_lifter_calibration_controller
+                controllers/gimbal_lifter_controller
+                controllers/gimbal_lifter_calibration_controller
+                controllers/arm_trajectory_controller
                 "
     />
+
+    <node pkg="tf2_ros" type="static_transform_publisher" name="odom_tf_publisher"
+          args="0 0 0 0 0 0 1 map odom"/>
 </launch>