Added the urdf, xacro and gazebo file for the pan&tilt unit.

Integrated the use of the pan&tilt unit in all CEABOT launch files.
Added the urdf, xacro and gazebo files for the IDS XS camera.
TEmporal chnages to try to redirect the printf output to a virtual serial device.

bioloid_apps/launch/ceabot/ceabot_base.launch

@@ -2,10 +2,12 @@
 
   <arg name="robot" default="bioloid_ceabot" />
   <arg name="mtn_file" default="bio_prm_humanoidtypea_en" />
+  <arg name="use_pan_tilt" default="false" />
 
   <include file="$(find bioloid_description)/launch/bioloid_cm510_sim.launch">
     <arg name="robot" value="$(arg robot)" />
     <arg name="mtn_file" value="$(arg mtn_file)" />
+    <arg name="use_pan_tilt" value="$(arg use_pan_tilt)" />
   </include>
 
   <!-- launch the action client node -->

bioloid_apps/launch/ceabot/ceabot_obstacles.launch

@@ -3,10 +3,12 @@
   <arg name="robot" default="bioloid_ceabot" />
   <arg name="environment" default="obstacles_env" />
   <arg name="mtn_file" default="bio_prm_humanoidtypea_en" />
+  <arg name="use_pan_tilt" default="false" />
 
   <include file="$(find bioloid_apps)/launch/ceabot/ceabot_base.launch">
     <arg name="robot" value="$(arg robot)"/>
     <arg name="mtn_file" value="$(arg mtn_file)"/>
+    <arg name="use_pan_tilt" value="$(arg use_pan_tilt)"/>
   </include>
 
   <include file="$(find bioloid_description)/launch/obstacles_env.launch">

bioloid_apps/launch/ceabot/ceabot_stairs.launch

@@ -3,10 +3,12 @@
   <arg name="robot" default="bioloid_ceabot" />
   <arg name="environment" default="stairs_env" />
   <arg name="mtn_file" default="bio_prm_humanoidtypea_en" />
+  <arg name="use_pan_tilt" default="false" />
 
   <include file="$(find bioloid_apps)/launch/ceabot/ceabot_base.launch">
     <arg name="robot" value="$(arg robot)"/>
     <arg name="mtn_file" value="$(arg mtn_file)"/>
+    <arg name="use_pan_tilt" value="$(arg use_pan_tilt)"/>
   </include>
 
   <include file="$(find bioloid_description)/launch/stairs_env.launch">

bioloid_apps/launch/ceabot/ceabot_vision.launch

@@ -3,10 +3,12 @@
   <arg name="robot" default="bioloid_ceabot" />
   <arg name="environment" default="vision_env" />
   <arg name="mtn_file" default="bio_prm_humanoidtypea_en" />
+  <arg name="use_pan_tilt" default="false" />
 
   <include file="$(find bioloid_apps)/launch/ceabot/ceabot_base.launch">
     <arg name="robot" value="$(arg robot)"/>
     <arg name="mtn_file" value="$(arg mtn_file)"/>
+    <arg name="use_pan_tilt" value="$(arg use_pan_tilt)"/>
   </include>
 
   <include file="$(find bioloid_description)/launch/vision_env.launch">

bioloid_control/config/bioloid_ceabot.yaml

@@ -18,6 +18,7 @@ bioloid:
     exp_gpio3_frame: right_front_fwd_ir_link
     exp_gpio4_frame: right_front_dwn_ir_link
     exp_gpio5_frame: right_lateral_dwn_ir_link
+    serial_console_device: /dev/ttyS100
     joints:
       - j_shoulder_r
       - j_shoulder_l

bioloid_control/config/bioloid_ceabot_pan_tilt.yaml

+bioloid:
+  # Publish all joint states -----------------------------------
+  joint_state_controller:
+    type: joint_state_controller/JointStateController
+    publish_rate: 50
+
+  bioloid_cm510_cont:
+    type: effort_controllers/BioloidControllerCM510
+    adc2_frame: IR1_link
+    adc3_frame: gyro_x
+    adc4_frame: gyro_y
+    exp_adc1_frame: IR2_link
+    exp_adc8_frame: IR3_link
+    exp_board_id: 192
+    exp_gpio0_frame: left_front_fwd_ir_link
+    exp_gpio1_frame: left_front_dwn_ir_link
+    exp_gpio2_frame: left_lateral_dwn_ir_link
+    exp_gpio3_frame: right_front_fwd_ir_link
+    exp_gpio4_frame: right_front_dwn_ir_link
+    exp_gpio5_frame: right_lateral_dwn_ir_link
+    pan_servo_id: 19
+    tilt_servo_id: 20
+    serial_console_device: /dev/pts/19
+    joints:
+      - j_shoulder_r
+      - j_shoulder_l
+      - j_high_arm_r
+      - j_high_arm_l
+      - j_low_arm_r
+      - j_low_arm_l
+      - j_pelvis_yaw_r
+      - j_pelvis_yaw_l
+      - j_pelvis_roll_r
+      - j_pelvis_roll_l
+      - j_pelvis_pitch_r
+      - j_pelvis_pitch_l
+      - j_knee_r
+      - j_knee_l
+      - j_ankle_pitch_r
+      - j_ankle_pitch_l
+      - j_ankle_roll_r
+      - j_ankle_roll_l
+      - j_pan
+      - j_tilt
+    gains:
+      j_shoulder_l:
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
+      j_high_arm_l:
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
+      j_low_arm_l:
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
+      j_shoulder_r:
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
+      j_high_arm_r:
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
+      j_low_arm_r:
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
+      j_pelvis_yaw_l:
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
+      j_pelvis_roll_l:
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
+      j_pelvis_pitch_l:
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
+      j_knee_l:
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
+      j_ankle_pitch_l:
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
+      j_ankle_roll_l:
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
+      j_pelvis_yaw_r:
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
+      j_pelvis_roll_r:
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
+      j_pelvis_pitch_r:
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
+      j_knee_r:
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
+      j_ankle_pitch_r:
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
+      j_ankle_roll_r:
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
+      j_pan:
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
+      j_tilt:
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5

bioloid_control/launch/bioloid_cm510_control.launch

@@ -2,9 +2,16 @@
 
   <arg name="mtn_file" default="bio_prm_humanoidtypea_en" />
   <arg name="robot" default="bioloid_ceabot" />
+  <arg name="use_pan_tilt" default="false" />
 
   <!-- Load joint controller configurations from YAML file to parameter server -->
-  <rosparam file="$(find bioloid_control)/config/$(arg robot).yaml" command="load"/>
+  <group if="$(arg use_pan_tilt)">
+    <rosparam file="$(find bioloid_control)/config/$(arg robot)_pan_tilt.yaml" command="load"/>
+  </group>
+
+  <group unless="$(arg use_pan_tilt)">
+    <rosparam file="$(find bioloid_control)/config/$(arg robot).yaml" command="load"/>
+  </group>
 
   <param name="motions_file" value="$(find bioloid_controller_cm510)/motions/$(arg mtn_file).mtn"/>
 

bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h

@@ -8,9 +8,11 @@
 
 /* firmare headers */
 #include "cm510.h"
+#include "dynamixel_master.h"
 #include "motion_manager.h"
 #include "motion_pages.h"
 #include "exp_board.h"
+#include "pan_tilt.h"
 #include "balance.h"
 #include "buzzer.h"
 #include "gpio.h"
@@ -54,14 +56,19 @@ extern "C" {
 }
 
 // expansion board external variables
-extern int exp_board_id;
+extern int exp_board_dyn_id;
 extern uint16_t exp_adc_values[EXP_NUM_ADC]; 
 extern uint16_t exp_compass_value; 
 extern uint8_t exp_gpio_values[EXP_NUM_GPIO]; 
+extern int pan_servo_dyn_id;
+extern int tilt_servo_dyn_id;
 extern "C" {
   extern void exp_board_loop(void);
 }
 
+// serial console external variables
+extern std::string serial_console_device;
+
 namespace bioloid_controller_cm510
 {
   namespace internal
@@ -251,9 +258,19 @@ namespace bioloid_controller_cm510
       this->init_exp_board_analog_ports(controller_nh);
       this->init_exp_board_gpio_ports(controller_nh);
       /* read the expansion board identifier */
-      exp_board_id=-1;
-      controller_nh_.getParam("exp_board_id", exp_board_id);
-      ROS_DEBUG_STREAM_NAMED(name_, "Expansion board identifier: " << exp_board_id);
+      exp_board_dyn_id=-1;
+      controller_nh_.getParam("exp_board_id", exp_board_dyn_id);
+      ROS_DEBUG_STREAM_NAMED(name_, "Expansion board identifier: " << exp_board_dyn_id);
+      pan_servo_dyn_id=-1;
+      controller_nh_.getParam("pan_servo_id", pan_servo_dyn_id);
+      ROS_DEBUG_STREAM_NAMED(name_, "Pan servo identifier: " << pan_servo_dyn_id);
+      tilt_servo_dyn_id=-1;
+      controller_nh_.getParam("tilt_servo_id", tilt_servo_dyn_id);
+      ROS_DEBUG_STREAM_NAMED(name_, "Tilt servo identifier: " << tilt_servo_dyn_id);
+
+      serial_console_device="";
+      controller_nh_.getParam("serial_console_device", serial_console_device);
+      ROS_DEBUG_STREAM_NAMED(name_, "Serial console device: " << serial_console_device);
 
       /* process the motion file parameters */
       try{
@@ -322,10 +339,11 @@ namespace bioloid_controller_cm510
         init_buzzer();
         init_user_time();
         /* call the user initialization function */
-        user_init();
+        dyn_master_init();
         manager_init(num_servos);
-        if(exp_board_id!=-1)
-          exp_board_init(exp_board_id);
+        if(exp_board_dyn_id!=-1)
+          exp_board_init(exp_board_dyn_id);
+        user_init();
         action_set_page(31);
         action_start_page();
         first=false;
@@ -350,8 +368,8 @@ namespace bioloid_controller_cm510
 
       for (unsigned int i = 0; i < this->joints_.size(); ++i)
       {
-        this->compliances_[i].slope=1<<(manager_servos[i].cc_slope&0x0F);
-        target_angles[i] = ((((manager_servos[i].current_value+balance_offsets[i]-manager_servos[i].center_value)*300.0/1023.0)*3.14159)/180.0);
+        this->compliances_[i].slope=1<<(manager_servos[i+1].slope&0x0F);
+        target_angles[i] = ((((manager_servos[i+1].current_value+balance_offsets[i+1]-manager_servos[i+1].center_value)*300.0/1023.0)*3.14159)/180.0);
         const double command = this->compliance_control(this->compliances_[i],target_angles[i]-real_angles[i]);
         this->joints_[i].setCommand(command);
       }
@@ -450,7 +468,7 @@ namespace bioloid_controller_cm510
            adc_values[ADC_PORT_6]=value;
       }
       /* handle the expansion board analog ports */
-      if(exp_board_id!=-1)
+      if(exp_board_dyn_id!=-1)
       {
         const double Vmax=5.0,Vmin=0.5;
         // compute the range value in volts
@@ -524,7 +542,6 @@ namespace bioloid_controller_cm510
         else if(frame_name==this->exp_gpio18_frame)
           exp_gpio_values[18]=threshold;
       }
-      std::cout << (int)exp_gpio_values[0] << "," << (int)exp_gpio_values[1] << "," << (int)exp_gpio_values[2] << "," << (int)exp_gpio_values[3] << "," << (int)exp_gpio_values[4] << "," << (int)exp_gpio_values[5] << std::endl;
     }
 
   template <class HardwareInterface>

bioloid_controller_cm510/include/sim/avr/io.h

@@ -3,8 +3,5 @@
 
 #include <inttypes.h>
 #include <avr/iomxx0_1.h>
-#include <stdio.h>
-
-FILE * fdevopen(int(*put)(char, FILE *),int(*get)(FILE *));
 
 #endif

bioloid_controller_cm510/src/sim/avr_registers.c

@@ -2,10 +2,6 @@
 #include <inttypes.h>
 #include <avr/io.h>
 
-FILE * fdevopen(int(*put)(char, FILE *),int(*get)(FILE *))
-{
-}
-
 uint8_t PINA;
 uint8_t DDRA;
 uint8_t PORTA;

bioloid_controller_cm510/src/sim/dynamixel_master.c

@@ -9,7 +9,10 @@
 /* external variables for the gazebo controller */
 int num_servos;
 double real_angles[PAGE_MAX_NUM_SERVOS];
-extern int exp_board_id;
+int exp_board_dyn_id;
+int pan_servo_dyn_id;
+int tilt_servo_dyn_id;
+
 extern uint16_t exp_adc_values[EXP_NUM_ADC];
 extern uint16_t exp_adc_avg_values[EXP_NUM_ADC];
 extern uint16_t exp_compass_value;
@@ -17,6 +20,7 @@ extern uint16_t exp_compass_avg_value;
 extern uint8_t exp_gpio_values[EXP_NUM_GPIO];
 
 /* private variables */
+uint8_t dyn_master_initialized=0x00;
 uint8_t dyn_master_tx_buffer[DYN_MASTER_MAX_TX_BUFFER_LEN];
 uint8_t dyn_master_sent_bytes;
 volatile uint8_t dyn_master_sent_done;
@@ -72,6 +76,12 @@ uint8_t dyn_master_wait_reception(void)
 /* public functions */
 void dyn_master_init(void)
 {
+  dyn_master_initialized=0x01;
+}
+
+uint8_t dyn_master_is_init(void)
+{
+  return dyn_master_initialized;
 }
 
 void dyn_master_set_rx_timeout(uint16_t timeout_us)
@@ -124,7 +134,7 @@ void dyn_master_scan(uint8_t *num,uint8_t *ids)
 
 uint8_t dyn_master_ping(uint8_t id)
 {
-  if((id!=0 && id<=num_servos) || id==(uint8_t)exp_board_id)
+  if((id!=0 && id<=num_servos) || id==(uint8_t)exp_board_dyn_id)
     return DYN_SUCCESS;
   else
     return DYN_COMM_ERROR;
@@ -132,10 +142,18 @@ uint8_t dyn_master_ping(uint8_t id)
 
 uint8_t dyn_master_read_byte(uint8_t id,uint16_t address,uint8_t *data)
 {
-  if(id==(uint8_t)exp_board_id)
+  if(id==(uint8_t)exp_board_dyn_id)
   {  
     if(address==Hardware_available)
       *data=0x03;// PWM and DAC present
+    else if(address==NUM_PWM_SERVOS)
+    {
+      *data=0;
+      if(pan_servo_dyn_id!=0xFF)
+        (*data)++;
+      if(tilt_servo_dyn_id!=0xFF)
+        (*data)++;
+    }  
   }
 
   return DYN_SUCCESS;
@@ -143,7 +161,7 @@ uint8_t dyn_master_read_byte(uint8_t id,uint16_t address,uint8_t *data)
 
 uint8_t dyn_master_read_word(uint8_t id,uint16_t address,uint16_t *data)
 {
-  if(id>=1 && id<=num_servos)
+  if(id>=1 && id<=18)
   {
     if(address==MODEL_NUM)
       *data=0x000C;
@@ -156,7 +174,20 @@ uint8_t dyn_master_read_word(uint8_t id,uint16_t address,uint16_t *data)
     else
       *data=0x0000;
   }
-  else if(id==(uint8_t)exp_board_id)
+  else if(id==(uint8_t)pan_servo_dyn_id || id==(uint8_t)tilt_servo_dyn_id)
+  {
+    if(address==MODEL_NUM)
+      *data=0x0C00;
+    else if(address==CURRENT_POS)
+      *data=((((real_angles[id-1]*180.0)/3.143159)+150.0)*1023.0)/300.0;
+    else if(address==CW_ANGLE_LIM)
+      *data=205;
+    else if(address==CCW_ANGLE_LIM)
+      *data=818;
+    else
+      *data=0x0000;
+  }
+  else if(id==(uint8_t)exp_board_dyn_id)
   {
     if(address==ADC_CHANNEL0_L)
       *data=exp_adc_values[0];
@@ -241,7 +272,7 @@ uint8_t dyn_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_
 {
   unsigned int i=0;
 
-  if(id==(uint8_t)exp_board_id)
+  if(id==(uint8_t)exp_board_dyn_id)
   {
     data[0]=exp_gpio_values[0];
     data[1]=exp_gpio_values[1];

bioloid_controller_cm510/src/sim/motion_pages.c

 #include "motion_pages.h"
-#include <stdio.h>
 
 TActionPage pages[MAX_PAGES];
 

bioloid_controller_cm510/src/sim/serial_console.c

-#include "comm_cfg.h"
-#include "serial_console.h"
-#include <stdio.h>
-
-/* private functions */
-void serial_console_set_baudrate(uint32_t baudrate)
-{
-}
-
-int serial_console_putchar(char c,FILE *dev)
-{
-}
-
-int serial_console_getchar(FILE *dev)
-{
-  return 0x00;
-}
-
-/* pubic functions */
-void serial_console_init(uint32_t baudrate)
-{
-}

bioloid_controller_cm510/src/sim/serial_console.cpp

+#include <iostream>
+#include <stdarg.h>
+#include "comm_cfg.h"
+#include "serial_console.h"
+#include "commexceptions.h"
+#include "rs232.h"
+
+extern "C" int vsprintf (char * s, const char * format, va_list arg );
+extern "C" int vfprintf ( FILE * stream, const char * format, va_list arg );
+extern FILE *stdout;
+
+// private variables
+CRS232 serial_console("bioloid_serial_port");
+std::string serial_console_device;
+TRS232_config serial_console_config;
+bool rs232_enabled;
+
+/* private functions */
+void serial_console_set_baudrate(uint32_t baudrate)
+{
+  serial_console_config.baud=baudrate;
+
+  try{
+    serial_console.config(&serial_console_config);
+  }catch(CCommException &e){
+    std::cout << e.what() << std::endl;
+  }
+}
+
+int serial_console_getchar(FILE *dev)
+{
+  CEventServer *event_server=CEventServer::instance();
+  std::list<std::string> events;
+  unsigned char byte;
+ 
+  events.push_back(serial_console.get_rx_event_id());
+  event_server->wait_first(events);
+  serial_console.read(&byte,1);
+  return byte;
+}
+
+/*int printf(const char *fmt, ...)
+{
+  va_list ap;
+  int i;
+
+  std::cout << "printf" << std::endl;
+  if(rs232_enabled)
+  {
+    char out_buff[256];
+    va_start(ap,fmt);
+    i=vsprintf(out_buff,fmt,ap);
+    va_end(ap);
+    serial_console.write((unsigned char *)out_buff,i);
+  }
+  else
+  {
+    va_start(ap,fmt);
+    i = vfprintf(stdout, fmt, ap);
+    va_end(ap);
+  }
+  return i;
+}*/
+
+/* pubic functions */
+void serial_console_init(uint32_t baudrate)
+{
+  serial_console_config.baud=baudrate;
+  serial_console_config.num_bits=8;
+  serial_console_config.parity=none;
+  serial_console_config.stop_bits=1;
+
+  try{
+    std::cout << serial_console_device << std::endl;
+    serial_console.open((void *)&serial_console_device);
+    serial_console.config(&serial_console_config);
+    rs232_enabled=true;
+  }catch(CCommException &e){
+    std::cout << e.what() << std::endl;
+    rs232_enabled=false;
+  }
+}

bioloid_description/launch/bioloid_base.launch

 <launch>
   <arg name="robot" default="bioloid" />
+  <arg name="use_pan_tilt" default="false" />
 
   <!-- Convert an xacro and put on parameter server -->
-  <param name="robot_description"
-         command="$(find xacro)/xacro.py '$(find bioloid_description)/urdf/$(arg robot).xacro'" />
+  <param name="robot_description" command="$(find xacro)/xacro.py '$(find bioloid_description)/urdf/$(arg robot).xacro' use_pan_tilt:=$(arg use_pan_tilt)"/>
 
   <node pkg="robot_state_publisher" type="state_publisher" name="bioloid_tf_broadcaster">
     <param name="tf_prefix" type="string" value="/bioloid"/>

bioloid_description/launch/bioloid_cm510_sim.launch

@@ -2,14 +2,17 @@
 
   <arg name="robot" default="bioloid_ceabot" />
   <arg name="mtn_file" default="bio_prm_humanoidtypea_en" />
+  <arg name="use_pan_tilt" default="false" />
 
   <include file="$(find bioloid_gazebo)/launch/bioloid_gazebo.launch">
     <arg name="robot" value="$(arg robot)"/>
+    <arg name="use_pan_tilt" value="$(arg use_pan_tilt)" />
   </include>
 
   <include file="$(find bioloid_control)/launch/bioloid_cm510_control.launch">
     <arg name="mtn_file" value="$(arg mtn_file)"/>
     <arg name="robot" value="$(arg robot)"/>
+    <arg name="use_pan_tilt" value="$(arg use_pan_tilt)" />
   </include>
 
 </launch>

bioloid_description/launch/bioloid_test.launch

 <launch>
 
   <arg name="robot" default="bioloid" /> 
+  <arg name="use_pan_tilt" default="false" /> 
 
   <!-- Convert an xacro and put on parameter server -->
   <param name="robot_description"
-         command="$(find xacro)/xacro.py '$(find bioloid_description)/urdf/$(arg robot).xacro'" />
+         command="$(find xacro)/xacro.py '$(find bioloid_description)/urdf/$(arg robot).xacro' use_pan_tilt:=$(arg use_pan_tilt)" />
 
   <node pkg="robot_state_publisher" type="state_publisher" name="bioloid_tf_broadcaster">
     <param name="tf_prefix" type="string" value="/bioloid"/>