[Model] Joint and tf now works

robots/mini_quadrotor/config/Servo.yaml

@@ -0,0 +1,33 @@
+servo_controller:
+  state_sub_topic: servo/states
+  ctrl_pub_topic: servo/target_states
+  torque_pub_topic: servo/torque_enable
+
+  joints:
+    angle_sgn: 1
+    angle_scale: 0.00076699
+    zero_point_offset: 2047
+    torque_scale: 0.03483 # convert to N*m
+
+    # for simulation
+    simulation:
+      pid: {p: 0.0, i: 0.0, d: 0}
+      init_value: 1.57
+      type: effort_controllers/JointPositionController
+
+    # should be in order in terms of kinematics model
+    # i.e. joint1 -> jointN
+    controller1:
+      id: 0
+      name: flex_joint_p
+      angle_sgn: -1
+      #angle_max: 1.58 # you can specify for each controller
+
+      #simulaton:  # you can specify for each controller
+        #type: effort_controllers/JointPositionController
+        #pid: {p: 50.0, i: 0.01, d: 2.0}
+        #init_value: 0
+
+    controller2:
+      id: 1
+      name: flex_joint_y

robots/mini_quadrotor/launch/bringup.launch

@@ -44,6 +44,9 @@
     ###########  Battery Config  ###########
     <rosparam file="$(arg config_dir)/Battery.yaml" command="load" />
 
+    ###########  Servo Config  ###########
+    <rosparam file="$(arg config_dir)/Servo.yaml" command="load" />    
+
     ###########  Control  ###########
     <rosparam file="$(arg config_dir)/LQIFlightControl.yaml" command="load" if="$(arg lqi_control_method)" />
     <rosparam file="$(arg config_dir)/QinvFlightControl.yaml" command="load" if="$(eval not arg('full_ctrl') and not arg('lqi_control_method'))"/>
@@ -75,7 +78,7 @@
     <arg name="robot_ns" value="$(arg robot_ns)" />
     <arg name="rviz_config" value="$(arg config_dir)/rviz_config" />
     <arg name="rviz_init_pose" value="$(arg config_dir)/RvizInit.yaml" />
-    <arg name="need_joint_state" value="true"/>
+    <arg name="need_joint_state" value="false" if ="$(eval arg('simulation') or arg('real_machine'))"/>
   </include>
 
   ###########  Sensors ###########
@@ -86,6 +89,10 @@
     <arg name="robot_id" value="$(arg robot_id)" />
   </include >
 
+  ###########  Servo Bridge  ###########
+  <node pkg="aerial_robot_model" type="servo_bridge_node" name="servo_bridge" ns="$(arg robot_ns)" output="screen" />
+
+
   ########## Simulation in Gazebo #########
   <include file="$(find aerial_robot_simulation)/launch/simulation.launch" if = "$(eval arg('simulation') and not arg('real_machine'))" >
     <arg name="robot_ns" default="$(arg robot_ns)" />