Implemented the custom printf and scanf functions to use the virtual serial port when it exists.

bioloid_apps/launch/ceabot/ceabot_base.launch

@@ -24,5 +24,16 @@
   <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
     output="screen"/>
 
+  <node name="gazebo_image_to_video_dev"
+        pkg="gazebo_image_to_video_dev"
+        type="gazebo_image_to_video_dev"
+        output="screen">
+    <remap from="~/camera_in/image_raw"
+           to="/bioloid/camera/image_raw"/>
+    <remap from="~/camera_in/camera_info"
+           to="/bioloid/camera/camera_info"/>
+    <param name="video_dev" value="/dev/video0"/>
+  </node>
+
 </launch>
 

bioloid_control/config/bioloid_ceabot.yaml

@@ -18,7 +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
+    serial_console_device: /dev/pts/18
     joints:
       - j_shoulder_r
       - j_shoulder_l

bioloid_control/config/bioloid_ceabot_pan_tilt.yaml

@@ -20,7 +20,7 @@ bioloid:
     exp_gpio5_frame: right_lateral_dwn_ir_link
     pan_servo_id: 19
     tilt_servo_id: 20
-    serial_console_device: /dev/pts/19
+    serial_console_device: /dev/pts/18
     joints:
       - j_shoulder_r
       - j_shoulder_l

bioloid_controller_cm510/CMakeLists.txt

@@ -92,7 +92,7 @@ catkin_package(
 ###########
 
 SET(BIOLOID_FW_PATH ~/humanoids/cm510_controller_fw)
-SET(ProjectPath ~/humanoids/cm510_controller_fw/examples/pan_tilt)
+SET(ProjectPath ~/humanoids/cm510_controller_fw/examples/vision)
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
@@ -130,7 +130,7 @@ add_library(${PROJECT_NAME} src/bioloid_controller_cm510.cpp
                             # communications modules
                             ${BIOLOID_FW_PATH}/communications/src/dynamixel.c
                             # main application module
-                            ${ProjectPath}/pan_tilt.c
+                            ${ProjectPath}/cm510_vision.c
                             # AVR simulation modules
                             src/sim/avr_delay.c
                             src/sim/avr_registers.c

bioloid_controller_cm510/src/sim/serial_console.cpp

@@ -6,8 +6,11 @@
 #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 "C" int vfprintf (FILE * stream, const char * format, va_list arg );
+extern "C" int vsscanf (const char * s, const char * format, va_list arg );
+extern "C" int vfscanf ( FILE * stream, const char * format, va_list arg );
 extern FILE *stdout;
+extern FILE *stdin;
 
 // private variables
 CRS232 serial_console("bioloid_serial_port");
@@ -39,12 +42,11 @@ int serial_console_getchar(FILE *dev)
   return byte;
 }
 
-/*int printf(const char *fmt, ...)
+int cm510_printf(const char *fmt, ...)
 {
   va_list ap;
   int i;
 
-  std::cout << "printf" << std::endl;
   if(rs232_enabled)
   {
     char out_buff[256];
@@ -60,7 +62,44 @@ int serial_console_getchar(FILE *dev)
     va_end(ap);
   }
   return i;
-}*/
+}
+
+int cm510_scanf(const char *fmt, ...)
+{
+  va_list ap;
+  int done,num;;
+
+  if(rs232_enabled)
+  {
+    char *out_buff=NULL;
+    CEventServer *event_server=CEventServer::instance();
+    std::list<std::string> events;
+
+    try{
+      events.push_back(serial_console.get_rx_event_id());
+      event_server->wait_first(events,1000);
+      num=serial_console.get_num_data();
+      out_buff=new char[num];
+      serial_console.read((unsigned char *)out_buff,num);
+      va_start(ap,fmt);
+      done=vsscanf(out_buff,fmt,ap);
+      va_end(ap);
+      delete[] out_buff;
+    }catch(CException &e){
+      std::cout << e.what() << std::endl;
+      if(out_buff!=NULL)
+        delete[] out_buff;
+    }
+  }
+  else
+  {
+    va_start (ap, fmt);
+    done = vfscanf (stdin, fmt, ap);
+    va_end (ap);
+  }
+  return done;
+
+}
 
 /* pubic functions */
 void serial_console_init(uint32_t baudrate)
@@ -80,3 +119,13 @@ void serial_console_init(uint32_t baudrate)
     rs232_enabled=false;
   }
 }
+
+uint8_t serial_console_get_num_data(void)
+{
+  if(rs232_enabled)
+    return serial_console.get_num_data();
+  else
+  {
+    return 0;
+  }
+}