diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
new file mode 100644
index 0000000..041c3ea
--- /dev/null
+++ b/.github/workflows/ci.yml
@@ -0,0 +1,25 @@
+name: ci
+
+on:
+  push:
+    branches:
+      - master
+  pull_request:
+    branches:
+      - master
+
+jobs:
+  build:
+    runs-on: ubuntu-20.04
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v3
+      - name: Setup ROS
+        uses: ros-tooling/setup-ros@v0.6
+        with:
+          required-ros-distributions: noetic
+      - name: Build
+        uses: ros-tooling/action-ros-ci@v0.3
+        with:
+          target-ros1-distro: noetic
+          vcs-repo-file-url: .rosinstall
diff --git a/.rosinstall b/.rosinstall
new file mode 100644
index 0000000..56f46b6
--- /dev/null
+++ b/.rosinstall
@@ -0,0 +1 @@
+repositories:
diff --git a/roomba_500driver_meiji/CMakeLists.txt b/roomba_500driver_meiji/CMakeLists.txt
index 54f00ff..2be9d74 100755
--- a/roomba_500driver_meiji/CMakeLists.txt
+++ b/roomba_500driver_meiji/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(roomba_500driver_meiji)
 
 ## Find catkin macros and libraries
@@ -181,11 +181,11 @@ add_dependencies(twist_to_roombactrl_converter ${PROJECT_NAME}_generate_messages
 # )
 
 ## Mark executables and/or libraries for installation
-# install(TARGETS roomba_500driver_meiji roomba_500driver_meiji_node
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
+install(TARGETS roomba_500driver_meiji main500 roomba500sci serial twist_to_roombactrl_converter
+  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}/
@@ -213,3 +213,10 @@ install(DIRECTORY include/${PROJECT_NAME}/
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(roslint REQUIRED)
+  set(ROSLINT_CPP_OPTS "--filter=-build/c++11,-runtime/references,-legal/copyright")
+  roslint_cpp()
+  roslint_add_test()
+endif()
diff --git a/roomba_500driver_meiji/include/roomba_500driver_meiji/roomba500sci.h b/roomba_500driver_meiji/include/roomba_500driver_meiji/roomba500sci.h
old mode 100755
new mode 100644
index 3742aa6..94d98ad
--- a/roomba_500driver_meiji/include/roomba_500driver_meiji/roomba500sci.h
+++ b/roomba_500driver_meiji/include/roomba_500driver_meiji/roomba500sci.h
@@ -1,227 +1,254 @@
-//
-//
-//
+// Copyright 2007-2023 amsl
 
-#ifndef _ROOMBASCI_H
-#define _ROOMBASCI_H
+#ifndef ROOMBA_500DRIVER_MEIJI_ROOMBA500SCI_H
+#define ROOMBA_500DRIVER_MEIJI_ROOMBA500SCI_H
 
-
-#include "serial.h"
+#include <algorithm>
 #include <sys/time.h>
 #include <unistd.h>
 
 #include <cmath>
 #include <roomba_500driver_meiji/Roomba500State.h>
+#include <roomba_500driver_meiji/serial.h>
 
-class Timer{
+class Timer
+{
 public:
-	void sleep(float sec){
-		long usec=(long)(sec*1000000);
-		usleep(usec);
-	}
+  void sleep(float sec)
+  {
+    const uint32_t usec = (uint32_t)(sec * 1000000u);
+    usleep(usec);
+  }
 };
 
-
-const float COMMAND_WAIT=0.01;	 // sec, this time is for Roomba 500 series
-const short DEFAULT_VELOCITY=200; // mm/s
+const float COMMAND_WAIT = 0.01;        // sec, this time is for Roomba 500 series
+const uint16_t DEFAULT_VELOCITY = 200;  // mm/s
 
 // DRIVE Special codes
-const short STRAIGHT_RADIUS=0x8000;
-const short TURN_CLOCK=-1;
-const short TURN_CNT_CLOCK=1;
-
+const uint16_t STRAIGHT_RADIUS = 0x8000;
+const int16_t TURN_CLOCK = -1;
+const int16_t TURN_CNT_CLOCK = 1;
 
-
-class roombaSci {
+class roombaSci
+{
 protected:
+  int receive(void);
+  int receive(unsigned char* pack, int byte);
 
-	int receive(void);
-	int receive(unsigned char* pack, int byte);
+  void packetToStruct(roomba_500driver_meiji::Roomba500State& ret, const unsigned char* pack);
 
-	void packetToStruct(roomba_500driver_meiji::Roomba500State& ret, const unsigned char* pack);
+  Serial* ser_;
+  unsigned char packet_[80];
 
-	Serial* ser_;
-	unsigned char packet_[80];
+  unsigned int enc_count_l_;
+  unsigned int enc_count_r_;
+  int d_enc_count_l_;
+  int d_enc_count_r_;
 
-	unsigned int enc_count_l_;
-	unsigned int enc_count_r_;
-	int d_enc_count_l_;
-	int d_enc_count_r_;
+  int d_pre_enc_l_;
+  int d_pre_enc_r_;
 
-	int d_pre_enc_l_;
-	int d_pre_enc_r_;
 public:
-
-	enum PACKET_ID{
-		GROUP_0=0,
-		GROUP_1=1,
-		GROUP_2=2,
-		GROUP_3=3,
-		GROUP_4=4,
-		GROUP_5=5,
-		GROUP_6=6,
-		GROUP_101=101,
-		GROUP_106=106,
-		GROUP_107=107,
-		ALL_PACKET=100};
-
-	enum OPCODE {
-		OC_START	= 128,
-		OC_BAUD		= 129,
-		OC_CONTROL	= 130,
-		OC_SAFE 	= 131,
-		OC_FULL 	= 132,
-		OC_POWER	= 133,
-		OC_SPOT 	= 134,
-		OC_CLEAN	= 135,
-		OC_MAX  	= 136,
-		OC_DRIVE	= 137,
-		OC_DRIVE_DIRECT	= 145,
-		OC_DRIVE_PWM	= 146,
-		OC_MOTORS	= 138,
-		OC_LEDS 	= 139,
-		OC_SONG 	= 140,
-		OC_PLAY  	= 141,
-		OC_SENSORS	= 142,
-		OC_QUERY_LIST=149,
-		OC_STREAM	= 148,
-		OC_PAUSE_STREAM = 150,
-		OC_FORCE_SEEKING_DOCK = 143,
-		OC_BUTTONS = 165
-	};
-
-	enum BAUD_CODE {
-		BC_300		= 0,
-		BC_600		= 1,
-		BC_1200		= 2,
-		BC_2400		= 3,
-		BC_4800		= 4,
-		BC_9600		= 5,
-		BC_14400	= 6,
-		BC_19200	= 7,
-		BC_28800	= 8,
-		BC_38400	= 9,
-		BC_57600	=10,
-		BC_115200	=11
-	};
-
-
-	enum MOTOR_BITS {
-		MB_MAIN_BRUSH = 0x04,
-		MB_VACUUM     = 0x02,
-		MB_SIDE_BRUSH = 0x01
-	};
-
-
-	enum LED_BITS {
-	};
-
-	enum PACKET_CODE {
-		PC_0 = 0,
-		PC_1 = 1,
-		PC_2 = 2,
-		PC_3 = 3,
-	};
-
-	enum BUTTONS_BIT{
-		BUTTON_CLEAN=0x01,
-		BUTTON_SPOT=0x02,
-		BUTTON_DOCK=0x04,
-		BUTTON_MINUTE=0x08,
-		BUTTON_HOUR=0x10,
-		BUTTON_DAY=0x20,
-		BUTTON_SCHEDULE=0x40,
-		BUTTON_CLOCK=0x80,
-	};
-
-	enum MOTOR{MOTOR_ON=1, MOTOR_OFF=0};
-	enum WALL{ NO_WALL=0, WALL_SEEN=1};
-	enum CLIRFF{ NO_CLIFF=0, CLIFF=1};
-	enum VWALL{NO_VWALL=0, VWALL_SEEN=1};
-	enum OVER_CUR{NO_OVER_C=0, OVER_C=1};
-	enum REMOTE_CC{NO_REMOTE=255};
-
-	enum BUMPS_WHEELDROPS {
-		WHEELDROP_CASTER = 0x10,
-		WHEELDROP_LEFT	 = 0x08,
-		WHEELDROP_RIGHT	 = 0x04,
-		BUMP_LEFT		= 0x02,
-		BUMP_RIGHT		= 0x01
-	};
-
-	enum MOTOR_OVERCURRENTS {
-		DRIVE_LEFT		= 0x10,
-		DRIVE_RIGHT		= 0x08,
-		MAIN_BRUSH		= 0x04,
-		VACUUM			= 0x02,
-		SIDE_BRUSH		= 0x01
-	};
-
-	enum BUTTONS {
-		B_POWER = 0x08,
-		B_SPOT  = 0x04,
-		B_CLEAN = 0x02,
-		B_MAX   = 0x01
-	};
-
-	enum CHARGING_STATE_CODES {
-		NOT_CHARGING		= 0,
-		CHARGING_RECOVERY	= 1,
-		CHARGING			= 2,
-		TRICKLE_CHARGING	= 3,
-		WAITING				= 4,
-		CHARGING_ERROR		= 5
-	};
-
-
-
-	roombaSci(int baud=B19200, const char* dev="/dev/ttyUSB0");
-	~roombaSci();
-
-	void wakeup(void);
-	void startup();
-	void powerOff();
-	void clean();
-	void safe();
-	void full();
-	void spot();
-	void max();
-	void dock();
-
-	void driveMotors(roombaSci::MOTOR_BITS state);
-	void forceSeekingDock();
-
-	void drive(short velocity, short radius);
-	void driveDirect(float velocity, float yawrate);
-	void drivePWM(int right_pwm,int left_pwm);
-
-	void song(unsigned char song_number, unsigned char song_length);
-	void playing(unsigned char song_number);
-
-	short velToPWMRight(float velocity);
-	short velToPWMLeft(float velocity);
-	float velToPWM(float velocity);
-
-	int sendOPCODE(roombaSci::OPCODE);
-	int getSensors();
-	int getSensors(roomba_500driver_meiji::Roomba500State& sensor);
-
-	int dEncoderRight(int max_delta=200){
-		d_enc_count_r_=std::max(-max_delta,d_enc_count_r_);
-		d_enc_count_r_=std::min(max_delta,d_enc_count_r_);
-		return d_enc_count_r_;
-	}
-
-	int dEncoderLeft(int max_delta=200){
-		d_enc_count_l_=std::max(-max_delta,d_enc_count_l_);
-		d_enc_count_l_=std::min(max_delta,d_enc_count_l_);
-		return d_enc_count_l_;
-	}
-
-	Timer* time_;
-};	// class
-
-
-
-#endif	// _ROOMBASCI_H
-
+  enum PACKET_ID
+  {
+    GROUP_0 = 0,
+    GROUP_1 = 1,
+    GROUP_2 = 2,
+    GROUP_3 = 3,
+    GROUP_4 = 4,
+    GROUP_5 = 5,
+    GROUP_6 = 6,
+    GROUP_101 = 101,
+    GROUP_106 = 106,
+    GROUP_107 = 107,
+    ALL_PACKET = 100
+  };
+
+  enum OPCODE
+  {
+    OC_START = 128,
+    OC_BAUD = 129,
+    OC_CONTROL = 130,
+    OC_SAFE = 131,
+    OC_FULL = 132,
+    OC_POWER = 133,
+    OC_SPOT = 134,
+    OC_CLEAN = 135,
+    OC_MAX = 136,
+    OC_DRIVE = 137,
+    OC_DRIVE_DIRECT = 145,
+    OC_DRIVE_PWM = 146,
+    OC_MOTORS = 138,
+    OC_LEDS = 139,
+    OC_SONG = 140,
+    OC_PLAY = 141,
+    OC_SENSORS = 142,
+    OC_QUERY_LIST = 149,
+    OC_STREAM = 148,
+    OC_PAUSE_STREAM = 150,
+    OC_FORCE_SEEKING_DOCK = 143,
+    OC_BUTTONS = 165
+  };
+
+  enum BAUD_CODE
+  {
+    BC_300 = 0,
+    BC_600 = 1,
+    BC_1200 = 2,
+    BC_2400 = 3,
+    BC_4800 = 4,
+    BC_9600 = 5,
+    BC_14400 = 6,
+    BC_19200 = 7,
+    BC_28800 = 8,
+    BC_38400 = 9,
+    BC_57600 = 10,
+    BC_115200 = 11
+  };
+
+  enum MOTOR_BITS
+  {
+    MB_MAIN_BRUSH = 0x04,
+    MB_VACUUM = 0x02,
+    MB_SIDE_BRUSH = 0x01
+  };
+
+  enum LED_BITS
+  {
+  };
+
+  enum PACKET_CODE
+  {
+    PC_0 = 0,
+    PC_1 = 1,
+    PC_2 = 2,
+    PC_3 = 3,
+  };
+
+  enum BUTTONS_BIT
+  {
+    BUTTON_CLEAN = 0x01,
+    BUTTON_SPOT = 0x02,
+    BUTTON_DOCK = 0x04,
+    BUTTON_MINUTE = 0x08,
+    BUTTON_HOUR = 0x10,
+    BUTTON_DAY = 0x20,
+    BUTTON_SCHEDULE = 0x40,
+    BUTTON_CLOCK = 0x80,
+  };
+
+  enum MOTOR
+  {
+    MOTOR_ON = 1,
+    MOTOR_OFF = 0
+  };
+  enum WALL
+  {
+    NO_WALL = 0,
+    WALL_SEEN = 1
+  };
+  enum CLIRFF
+  {
+    NO_CLIFF = 0,
+    CLIFF = 1
+  };
+  enum VWALL
+  {
+    NO_VWALL = 0,
+    VWALL_SEEN = 1
+  };
+  enum OVER_CUR
+  {
+    NO_OVER_C = 0,
+    OVER_C = 1
+  };
+  enum REMOTE_CC
+  {
+    NO_REMOTE = 255
+  };
+
+  enum BUMPS_WHEELDROPS
+  {
+    WHEELDROP_CASTER = 0x10,
+    WHEELDROP_LEFT = 0x08,
+    WHEELDROP_RIGHT = 0x04,
+    BUMP_LEFT = 0x02,
+    BUMP_RIGHT = 0x01
+  };
+
+  enum MOTOR_OVERCURRENTS
+  {
+    DRIVE_LEFT = 0x10,
+    DRIVE_RIGHT = 0x08,
+    MAIN_BRUSH = 0x04,
+    VACUUM = 0x02,
+    SIDE_BRUSH = 0x01
+  };
+
+  enum BUTTONS
+  {
+    B_POWER = 0x08,
+    B_SPOT = 0x04,
+    B_CLEAN = 0x02,
+    B_MAX = 0x01
+  };
+
+  enum CHARGING_STATE_CODES
+  {
+    NOT_CHARGING = 0,
+    CHARGING_RECOVERY = 1,
+    CHARGING = 2,
+    TRICKLE_CHARGING = 3,
+    WAITING = 4,
+    CHARGING_ERROR = 5
+  };
+
+  explicit roombaSci(int baud = B19200, const char* dev = "/dev/ttyUSB0");
+  ~roombaSci();
+
+  void wakeup(void);
+  void startup();
+  void powerOff();
+  void clean();
+  void safe();
+  void full();
+  void spot();
+  void max();
+  void dock();
+
+  void driveMotors(roombaSci::MOTOR_BITS state);
+  void forceSeekingDock();
+
+  void drive(int16_t velocity, int16_t radius);
+  void driveDirect(float velocity, float yawrate);
+  void drivePWM(int right_pwm, int left_pwm);
+
+  void song(unsigned char song_number, unsigned char song_length);
+  void playing(unsigned char song_number);
+
+  int16_t velToPWMRight(float velocity);
+  int16_t velToPWMLeft(float velocity);
+  float velToPWM(float velocity);
+
+  int sendOPCODE(roombaSci::OPCODE);
+  int getSensors();
+  int getSensors(roomba_500driver_meiji::Roomba500State& sensor);
+
+  int dEncoderRight(int max_delta = 200)
+  {
+    d_enc_count_r_ = std::max(-max_delta, d_enc_count_r_);
+    d_enc_count_r_ = std::min(max_delta, d_enc_count_r_);
+    return d_enc_count_r_;
+  }
+
+  int dEncoderLeft(int max_delta = 200)
+  {
+    d_enc_count_l_ = std::max(-max_delta, d_enc_count_l_);
+    d_enc_count_l_ = std::min(max_delta, d_enc_count_l_);
+    return d_enc_count_l_;
+  }
+
+  Timer* time_;
+};  // class
+
+#endif  // ROOMBA_500DRIVER_MEIJI_ROOMBA500SCI_H
diff --git a/roomba_500driver_meiji/include/roomba_500driver_meiji/serial.h b/roomba_500driver_meiji/include/roomba_500driver_meiji/serial.h
old mode 100755
new mode 100644
index d210eff..bf47f47
--- a/roomba_500driver_meiji/include/roomba_500driver_meiji/serial.h
+++ b/roomba_500driver_meiji/include/roomba_500driver_meiji/serial.h
@@ -16,8 +16,8 @@
  */
 //-----------------------------------------------------------------------------
 
-#ifndef _SERIAL_H
-#define _SERIAL_H
+#ifndef ROOMBA_500DRIVER_MEIJI_SERIAL_H
+#define ROOMBA_500DRIVER_MEIJI_SERIAL_H
 
 #include <termios.h>
 
@@ -28,26 +28,22 @@
 #define FALSE 0
 #define TRUE 1
 
-//#define DEBUG
+// #define DEBUG
 
 class Serial
 {
 private:
-
-	int fd_;//, c_, res_;
-	struct termios oldtio_, newtio_;
-
+  int fd_;  //, c_, res_;
+  struct termios oldtio_, newtio_;
 
 public:
+  Serial(int baudrate, const char* modemdevice, int vmin = 0, int lflag = 0);
+  ~Serial();
 
-	Serial(int baudrate, const char* modemdevice, int vmin=0, int lflag=0);
-	~Serial();
-
-	int read(unsigned char* p, int len);
-	int write(const unsigned char* p, int len);
-	void setVmin(int vmin);  // non canonical 時のreadで待つ最低限の文字数
-	void setRts(int);
-
+  int read(unsigned char* p, int len);
+  int write(const unsigned char* p, int len);
+  void setVmin(int vmin);  // non canonical 時のreadで待つ最低限の文字数
+  void setRts(int);
 };
 
-#endif //_SERIAL_H
+#endif  // ROOMBA_500DRIVER_MEIJI_SERIAL_H
diff --git a/roomba_500driver_meiji/package.xml b/roomba_500driver_meiji/package.xml
index 0b3c834..ec8d136 100755
--- a/roomba_500driver_meiji/package.xml
+++ b/roomba_500driver_meiji/package.xml
@@ -52,6 +52,8 @@
   <run_depend>sensor_msgs</run_depend>
   <run_depend>tf</run_depend>
   <run_depend>message_runtime</run_depend>
+  <test_depend>rostest</test_depend>
+  <test_depend>roslint</test_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
diff --git a/roomba_500driver_meiji/src/main500.cpp b/roomba_500driver_meiji/src/main500.cpp
old mode 100755
new mode 100644
index 929b99f..d53292c
--- a/roomba_500driver_meiji/src/main500.cpp
+++ b/roomba_500driver_meiji/src/main500.cpp
@@ -1,6 +1,7 @@
+// Copyright 2007-2023 amsl
 #include <ros/ros.h>
 
-#include "roomba_500driver_meiji/roomba500sci.h"
+#include <roomba_500driver_meiji/roomba500sci.h>
 #include <roomba_500driver_meiji/Roomba500State.h>
 #include <roomba_500driver_meiji/RoombaCtrl.h>
 
@@ -12,262 +13,301 @@
 boost::mutex cntl_mutex_;
 
 #include <iostream>
+#include <string>
 #include <math.h>
-using namespace std;
 
-//2013.10.09...tfのodomをコメントアウト
+// 2013.10.09...tfのodomをコメントアウト
 
 roombaSci* roomba;
 roomba_500driver_meiji::RoombaCtrl roombactrl;
 
-void cntl_callback(const roomba_500driver_meiji::RoombaCtrlConstPtr& msg){
-	roombactrl = *msg;
-	boost::mutex::scoped_lock(cntl_mutex_);
-
-	switch(msg->mode){
-		case roomba_500driver_meiji::RoombaCtrl::SPOT:
-			roomba->spot();
-			break;
-
-		case roomba_500driver_meiji::RoombaCtrl::SAFE:
-			roomba->safe();
-			break;
-
-		case roomba_500driver_meiji::RoombaCtrl::CLEAN:
-			roomba->clean();
-			break;
-
-		case roomba_500driver_meiji::RoombaCtrl::POWER:
-			roomba->powerOff();
-			break;
-
-		case roomba_500driver_meiji::RoombaCtrl::WAKEUP:
-			roomba->wakeup();
-			roomba->startup();
-			break;
-
-		case roomba_500driver_meiji::RoombaCtrl::FULL:
-			roomba->full();
-			break;
-
-		case roomba_500driver_meiji::RoombaCtrl::MAX:
-			roomba->max();
-			break;
-
-		case roomba_500driver_meiji::RoombaCtrl::DOCK:
-			roomba->dock();
-			break;
-
-		case roomba_500driver_meiji::RoombaCtrl::MOTORS:
-			roomba->driveMotors((roombaSci::MOTOR_BITS)(roombaSci::MB_MAIN_BRUSH | roombaSci::MB_SIDE_BRUSH | roombaSci::MB_VACUUM));
-			break;
-
-		case roomba_500driver_meiji::RoombaCtrl::MOTORS_OFF:
-			roomba->driveMotors((roombaSci::MOTOR_BITS)(0));
-			break;
-
-		case roomba_500driver_meiji::RoombaCtrl::DRIVE_DIRECT:
-			roomba->driveDirect(msg->cntl.linear.x, msg->cntl.angular.z);
-			break;
-
-		case roomba_500driver_meiji::RoombaCtrl::DRIVE_PWM:
-			roomba->drivePWM(msg->r_pwm, msg->l_pwm);
-			break;
-
-		case roomba_500driver_meiji::RoombaCtrl::SONG:
-			roomba->safe();
-			roomba->song(1,1);
-			roomba->playing(1);
-			break;
-
-		case roomba_500driver_meiji::RoombaCtrl::DRIVE:
-		default:
-			roomba->drive(msg->velocity, msg->radius);
-
-	}
+void cntl_callback(const roomba_500driver_meiji::RoombaCtrlConstPtr& msg)
+{
+  roombactrl = *msg;
+  boost::mutex::scoped_lock(cntl_mutex_);
+
+  switch (msg->mode)
+  {
+    case roomba_500driver_meiji::RoombaCtrl::SPOT:
+      roomba->spot();
+      break;
+
+    case roomba_500driver_meiji::RoombaCtrl::SAFE:
+      roomba->safe();
+      break;
+
+    case roomba_500driver_meiji::RoombaCtrl::CLEAN:
+      roomba->clean();
+      break;
+
+    case roomba_500driver_meiji::RoombaCtrl::POWER:
+      roomba->powerOff();
+      break;
+
+    case roomba_500driver_meiji::RoombaCtrl::WAKEUP:
+      roomba->wakeup();
+      roomba->startup();
+      break;
+
+    case roomba_500driver_meiji::RoombaCtrl::FULL:
+      roomba->full();
+      break;
+
+    case roomba_500driver_meiji::RoombaCtrl::MAX:
+      roomba->max();
+      break;
+
+    case roomba_500driver_meiji::RoombaCtrl::DOCK:
+      roomba->dock();
+      break;
+
+    case roomba_500driver_meiji::RoombaCtrl::MOTORS:
+      roomba->driveMotors(
+          (roombaSci::MOTOR_BITS)(roombaSci::MB_MAIN_BRUSH | roombaSci::MB_SIDE_BRUSH | roombaSci::MB_VACUUM));
+      break;
+
+    case roomba_500driver_meiji::RoombaCtrl::MOTORS_OFF:
+      roomba->driveMotors((roombaSci::MOTOR_BITS)(0));
+      break;
+
+    case roomba_500driver_meiji::RoombaCtrl::DRIVE_DIRECT:
+      roomba->driveDirect(msg->cntl.linear.x, msg->cntl.angular.z);
+      break;
+
+    case roomba_500driver_meiji::RoombaCtrl::DRIVE_PWM:
+      roomba->drivePWM(msg->r_pwm, msg->l_pwm);
+      break;
+
+    case roomba_500driver_meiji::RoombaCtrl::SONG:
+      roomba->safe();
+      roomba->song(1, 1);
+      roomba->playing(1);
+      break;
+
+    case roomba_500driver_meiji::RoombaCtrl::DRIVE:
+    default:
+      roomba->drive(msg->velocity, msg->radius);
+  }
 }
 
-void printSensors(const roomba_500driver_meiji::Roomba500State& sens){
-
-	cout<<"\n\n-------------------"<<endl;
-
-	//Roombaのセンサデータを出力しています
-	//必要ない場合はコメントアウトしてください
-
-	cout<<"bumps : "<<(bool)sens.bump.right<<"  "<<(bool)sens.bump.left<<endl;
-	cout<<"wheeldrops : "<<(bool)sens.wheeldrop.right<<"  "<<(bool)sens.wheeldrop.left<<"  "<<(bool)sens.wheeldrop.caster<<endl;
-	cout<<"wall : "<<(bool)sens.wall<<endl;
-	cout<<"cliff : "<<(bool)sens.cliff.left<<" "<<(bool)sens.cliff.right<<endl;
-	cout<<"virtual wall : "<<(bool)sens.virtual_wall<<endl;
-	cout<<"motor ovc : "
-		<<sens.motor_overcurrents.side_brush<<" "
-		<<sens.motor_overcurrents.vacuum<<" "
-		<<sens.motor_overcurrents.main_brush<<" "
-		<<sens.motor_overcurrents.drive_right<<" "
-		<<sens.motor_overcurrents.drive_left<<" "
-		<<endl;
-
-	cout<<"dirt_detector : "<<(short)sens.dirt_detector.left<<" "<<(short)sens.dirt_detector.right<<endl;
-	cout<<"remote control command : "<<(short)sens.remote_control_command<<endl;
-	cout<<"buttons : "<<(bool)sens.buttons.power<<" "<<(bool)sens.buttons.spot<<" "<<(bool)sens.buttons.clean<<" "<<(bool)sens.buttons.max<<endl;
-
-	cout<<"charging_state : "<<(short)sens.charging_state<<endl;
-	cout<<"voltage : "<<sens.voltage<<endl;
-	cout<<"current : "<<sens.current<<endl;
-	cout<<"temperature : "<<(short)sens.temperature<<endl;
-	cout<<"charge : "<<sens.charge<<endl;
-	cout<<"capacity : "<<sens.capacity<<endl;
-
+void printSensors(const roomba_500driver_meiji::Roomba500State& sens)
+{
+  std::cout << "\n\n-------------------" << std::endl;
+
+  // Roombaのセンサデータを出力しています
+  // 必要ない場合はコメントアウトしてください
+
+  std::cout << "bumps : "
+            << static_cast<bool>(sens.bump.right)
+            << "  "
+            << static_cast<bool>(sens.bump.left) << std::endl;
+  std::cout << "wheeldrops : "
+            << static_cast<bool>(sens.wheeldrop.right)
+            << "  "
+            << static_cast<bool>(sens.wheeldrop.left)
+            << "  "
+            << static_cast<bool>(sens.wheeldrop.caster) << std::endl;
+  std::cout << "wall : " << static_cast<bool>(sens.wall) << std::endl;
+  std::cout << "cliff : "
+            << static_cast<bool>(sens.cliff.left)
+            << " "
+            << static_cast<bool>(sens.cliff.right)
+            << std::endl;
+  std::cout << "virtual wall : " << static_cast<bool>(sens.virtual_wall) << std::endl;
+  std::cout << "motor ovc : "
+            << sens.motor_overcurrents.side_brush << " "
+            << sens.motor_overcurrents.vacuum << " "
+            << sens.motor_overcurrents.main_brush << " "
+            << sens.motor_overcurrents.drive_right << " "
+            << sens.motor_overcurrents.drive_left << " "
+            << std::endl;
+
+  std::cout << "dirt_detector : "
+            << static_cast<int16_t>(sens.dirt_detector.left)
+            << " "
+            << static_cast<int16_t>(sens.dirt_detector.right)
+            << std::endl;
+  std::cout << "remote control command : " << static_cast<int16_t>(sens.remote_control_command) << std::endl;
+  std::cout << "buttons : "
+            << static_cast<bool>(sens.buttons.power)
+            << " "
+            << static_cast<bool>(sens.buttons.spot)
+            << " "
+            << static_cast<bool>(sens.buttons.clean)
+            << " "
+            << static_cast<bool>(sens.buttons.max)
+            << std::endl;
+
+  std::cout << "charging_state : " << static_cast<int16_t>(sens.charging_state) << std::endl;
+  std::cout << "voltage : " << sens.voltage << std::endl;
+  std::cout << "current : " << sens.current << std::endl;
+  std::cout << "temperature : " << static_cast<int16_t>(sens.temperature) << std::endl;
+  std::cout << "charge : " << sens.charge << std::endl;
+  std::cout << "capacity : " << sens.capacity << std::endl;
 }
 
-double piToPI(double rad){
-	double ret=rad;
+double piToPI(double rad)
+{
+  double ret = rad;
 
-	if(rad>M_PI){
-		ret=rad-2.0*M_PI;
-	}
+  if (rad > M_PI)
+  {
+    ret = rad - 2.0 * M_PI;
+  }
 
-	if(rad<-M_PI){
-		ret=rad+2.0*M_PI;
-	}
+  if (rad < -M_PI)
+  {
+    ret = rad + 2.0 * M_PI;
+  }
 
-	return ret;
+  return ret;
 }
 
 void calcOdometry(
-	geometry_msgs::Pose2D& x,
-	const geometry_msgs::Pose2D& pre_x,
-	float dist,
-	float angle)
+    geometry_msgs::Pose2D& x,
+    const geometry_msgs::Pose2D& pre_x,
+    float dist,
+    float angle)
 {
-
-	x.theta=pre_x.theta+angle;
-	x.theta=piToPI(x.theta);
-	x.x=pre_x.x+dist*cos(x.theta);
-	x.y=pre_x.y+dist*sin(x.theta);
+  x.theta = pre_x.theta + angle;
+  x.theta = piToPI(x.theta);
+  x.x = pre_x.x + dist * cos(x.theta);
+  x.y = pre_x.y + dist * sin(x.theta);
 }
 
-int main(int argc, char** argv) {
-
-	ros::init(argc, argv, "roomba_driver");
-	ros::NodeHandle n;
-	ros::NodeHandle private_nh("~");
-
-	ros::Subscriber cntl_sub = n.subscribe("/roomba/control", 100, cntl_callback);
-
-	ros::Publisher pub_state=n.advertise<roomba_500driver_meiji::Roomba500State>("/roomba/states", 100);
-
-	tf::TransformBroadcaster odom_broadcaster;
-
-	ros::Publisher pub_odo= n.advertise<nav_msgs::Odometry >("/roomba/odometry", 100);
-
-    std::string USB_PORT;
-	private_nh.param("USB_PORT", USB_PORT, std::string("/dev/ttyUSB0"));
-	roomba = new roombaSci(B115200, USB_PORT.c_str());
-	roomba->wakeup();
-	roomba->startup();
-
-    std::string ROBOT_FRAME;
-	private_nh.param("ROBOT_FRAME", ROBOT_FRAME, std::string("base_link"));
-    std::string ODOM_FRAME;
-	private_nh.param("ODOM_FRAME", ODOM_FRAME, std::string("odom"));
-
-	ros::Rate loop_rate(10.0); // should not set this rate more than 20Hz !
-
-	geometry_msgs::Pose2D pose;
-	pose.x=0;	pose.y=0;	pose.theta=0;
-
-	int pre_enc_r=0,pre_enc_l=0;
-
-	ros::Time current_time, last_time;
-	current_time = ros::Time::now();
-	last_time = ros::Time::now();
-	while (ros::ok()) {
-        	current_time = ros::Time::now();
-
-		roomba_500driver_meiji::Roomba500State sens;
-		sens.header.stamp=ros::Time::now();
-
-		{
-		boost::mutex::scoped_lock(cntl_mutex_);
-		roomba->getSensors(sens);
-		}
-		//printSensors(sens);
-
-		int enc_r=roomba->dEncoderRight();
-		if(abs(enc_r)==200){
-			enc_r=pre_enc_r;
-		}
-		int enc_l=roomba->dEncoderLeft();
-		if(abs(enc_l)==200){
-			enc_l=pre_enc_l;
-		}
-
-
-		geometry_msgs::Pose2D pre=pose;
-
-		float distance=(float)((float)enc_r+(float)enc_l)/2270.0*0.5;
-		float angle=(float)((float)enc_r-(float)enc_l)/2270.0/0.235;
-		sens.distance=(short)(1000*distance);
-		sens.angle=(short)(angle*180.0/M_PI);
-
-		pub_state.publish(sens);
-
-		calcOdometry(pose, pre, distance, angle);
-
-		pre_enc_r=roomba->dEncoderRight();
-		pre_enc_l=roomba->dEncoderLeft();
-
-		//since all odometry is 6DOF we'll need a quaternion created from yaw
-		//ROSのOdometryには，6DOFを利用するのでyaw角から生成したquaternionを用いる
-		geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(pose.theta);
-
-		//first, we'll publish the transform over tf
-		geometry_msgs::TransformStamped odom_trans;
-		odom_trans.header.stamp = current_time;
-		odom_trans.header.frame_id = ODOM_FRAME;	//2013.10.09
-		odom_trans.child_frame_id = ROBOT_FRAME;	//2013.10.09
-
-		odom_trans.transform.translation.x = pose.x;
-		odom_trans.transform.translation.y = pose.y;
-		odom_trans.transform.translation.z = 0.0;
-		odom_trans.transform.rotation = odom_quat;
-
-		//send the transform
-		odom_broadcaster.sendTransform(odom_trans);	//2013.10.09
-
-		//next, we'll publish the odometry message over ROS
-		nav_msgs::Odometry odom;
-		odom.header.stamp = current_time;
-		odom.header.frame_id = ODOM_FRAME; //2013.10.09
-
-		//set the position
-		odom.pose.pose.position.x = pose.x;
-		odom.pose.pose.position.y = pose.y;
-		odom.pose.pose.position.z = 0.0;
-		odom.pose.pose.orientation = odom_quat;
-
-		//set the velocity
-		odom.child_frame_id = ROBOT_FRAME;	//2013.10.09
-		odom.twist.twist.linear.x = roombactrl.cntl.linear.x;
-		odom.twist.twist.linear.y = 0;
-		odom.twist.twist.angular.z = roombactrl.cntl.angular.z;
-
-
-		pub_odo.publish(odom);	//2013.10.09
-
-		last_time = current_time;
-
-		ros::spinOnce();
-		loop_rate.sleep();
-	        ROS_INFO("dt:%f\t444444444l: %5d\tr:%5d\tdl %4d\tdr %4d\tx:%f\ty:%f\ttheta:%f", last_time.toSec()-current_time.toSec(), sens.encoder_counts.left, sens.encoder_counts.right,  roomba->dEncoderLeft(), roomba->dEncoderRight(), pose.x,pose.y,pose.theta/M_PI*180.0);
-	}
-
-	roomba->powerOff();
-
-	roomba->time_->sleep(1);
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "roomba_driver");
+  ros::NodeHandle n;
+  ros::NodeHandle private_nh("~");
+
+  ros::Subscriber cntl_sub = n.subscribe("/roomba/control", 100, cntl_callback);
+
+  ros::Publisher pub_state = n.advertise<roomba_500driver_meiji::Roomba500State>("/roomba/states", 100);
+
+  tf::TransformBroadcaster odom_broadcaster;
 
-	delete roomba;
+  ros::Publisher pub_odo = n.advertise<nav_msgs::Odometry>("/roomba/odometry", 100);
 
-	return 0;
+  std::string USB_PORT;
+  private_nh.param("USB_PORT", USB_PORT, std::string("/dev/ttyUSB0"));
+  roomba = new roombaSci(B115200, USB_PORT.c_str());
+  roomba->wakeup();
+  roomba->startup();
+
+  std::string ROBOT_FRAME;
+  private_nh.param("ROBOT_FRAME", ROBOT_FRAME, std::string("base_link"));
+  std::string ODOM_FRAME;
+  private_nh.param("ODOM_FRAME", ODOM_FRAME, std::string("odom"));
+
+  ros::Rate loop_rate(10.0);  // should not set this rate more than 20Hz !
+
+  geometry_msgs::Pose2D pose;
+  pose.x = 0;
+  pose.y = 0;
+  pose.theta = 0;
+
+  int pre_enc_r = 0, pre_enc_l = 0;
+
+  ros::Time current_time, last_time;
+  current_time = ros::Time::now();
+  last_time = ros::Time::now();
+  while (ros::ok())
+  {
+    current_time = ros::Time::now();
+
+    roomba_500driver_meiji::Roomba500State sens;
+    sens.header.stamp = ros::Time::now();
+
+    {
+      boost::mutex::scoped_lock(cntl_mutex_);
+      roomba->getSensors(sens);
+    }
+    // printSensors(sens);
+
+    int enc_r = roomba->dEncoderRight();
+    if (abs(enc_r) == 200)
+    {
+      enc_r = pre_enc_r;
+    }
+    int enc_l = roomba->dEncoderLeft();
+    if (abs(enc_l) == 200)
+    {
+      enc_l = pre_enc_l;
+    }
+
+    geometry_msgs::Pose2D pre = pose;
+
+    const float distance = static_cast<float>(enc_r + enc_l) / 2270.0 * 0.5;
+    const float angle = static_cast<float>(enc_r - enc_l) / 2270.0 / 0.235;
+    sens.distance = static_cast<int16_t>(1000 * distance);
+    sens.angle = static_cast<int16_t>(angle * 180.0 / M_PI);
+
+    pub_state.publish(sens);
+
+    calcOdometry(pose, pre, distance, angle);
+
+    pre_enc_r = roomba->dEncoderRight();
+    pre_enc_l = roomba->dEncoderLeft();
+
+    // since all odometry is 6DOF we'll need a quaternion created from yaw
+    // ROSのOdometryには，6DOFを利用するのでyaw角から生成したquaternionを用いる
+    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(pose.theta);
+
+    // first, we'll publish the transform over tf
+    geometry_msgs::TransformStamped odom_trans;
+    odom_trans.header.stamp = current_time;
+    odom_trans.header.frame_id = ODOM_FRAME;  // 2013.10.09
+    odom_trans.child_frame_id = ROBOT_FRAME;  // 2013.10.09
+
+    odom_trans.transform.translation.x = pose.x;
+    odom_trans.transform.translation.y = pose.y;
+    odom_trans.transform.translation.z = 0.0;
+    odom_trans.transform.rotation = odom_quat;
+
+    // send the transform
+    odom_broadcaster.sendTransform(odom_trans);  // 2013.10.09
+
+    // next, we'll publish the odometry message over ROS
+    nav_msgs::Odometry odom;
+    odom.header.stamp = current_time;
+    odom.header.frame_id = ODOM_FRAME;  // 2013.10.09
+
+    // set the position
+    odom.pose.pose.position.x = pose.x;
+    odom.pose.pose.position.y = pose.y;
+    odom.pose.pose.position.z = 0.0;
+    odom.pose.pose.orientation = odom_quat;
+
+    // set the velocity
+    odom.child_frame_id = ROBOT_FRAME;  // 2013.10.09
+    odom.twist.twist.linear.x = roombactrl.cntl.linear.x;
+    odom.twist.twist.linear.y = 0;
+    odom.twist.twist.angular.z = roombactrl.cntl.angular.z;
+
+    pub_odo.publish(odom);  // 2013.10.09
+
+    last_time = current_time;
+
+    ros::spinOnce();
+    loop_rate.sleep();
+    ROS_INFO(
+        "dt:%f\t444444444l: %5d\tr:%5d\tdl %4d\tdr %4d\tx:%f\ty:%f\ttheta:%f",
+        last_time.toSec() - current_time.toSec(),
+        sens.encoder_counts.left,
+        sens.encoder_counts.right,
+        roomba->dEncoderLeft(),
+        roomba->dEncoderRight(),
+        pose.x,
+        pose.y,
+        pose.theta / M_PI * 180.0);
+  }
+
+  roomba->powerOff();
+
+  roomba->time_->sleep(1);
+
+  delete roomba;
+
+  return 0;
 }
diff --git a/roomba_500driver_meiji/src/roomba500sci.cpp b/roomba_500driver_meiji/src/roomba500sci.cpp
old mode 100755
new mode 100644
index db846d6..004c372
--- a/roomba_500driver_meiji/src/roomba500sci.cpp
+++ b/roomba_500driver_meiji/src/roomba500sci.cpp
@@ -1,342 +1,366 @@
-//
-//
-//
+// Copyright 2007-2023 amsl
 
 #include "ros/ros.h"
 #include "roomba_500driver_meiji/roomba500sci.h"
 
 #include <iostream>
 #include <cmath>
-using namespace std;
 
 roombaSci::roombaSci(int baud, const char* dev)
-:enc_count_l_(0),enc_count_r_(0),
-d_enc_count_l_(0),d_enc_count_r_(0),
-d_pre_enc_l_(0), d_pre_enc_r_(0){
-	ser_ = new Serial(baud,dev,80,0);
-	time_= new Timer();
-
-	time_->sleep(1);
-	roomba_500driver_meiji::Roomba500State sensor;
-	getSensors(sensor);
+  : enc_count_l_(0)
+  , enc_count_r_(0)
+  , d_enc_count_l_(0)
+  , d_enc_count_r_(0)
+  , d_pre_enc_l_(0)
+  , d_pre_enc_r_(0)
+{
+  ser_ = new Serial(baud, dev, 80, 0);
+  time_ = new Timer();
 
+  time_->sleep(1);
+  roomba_500driver_meiji::Roomba500State sensor;
+  getSensors(sensor);
 }
 
 roombaSci::~roombaSci()
 {
-	delete ser_;
-	delete time_;
-
+  delete ser_;
+  delete time_;
 }
 
-
 void roombaSci::wakeup(void)
 {
-	ser_->setRts(0);
-	time_->sleep(0.1);
-	ser_->setRts(1);
-	time_->sleep(COMMAND_WAIT);
+  ser_->setRts(0);
+  time_->sleep(0.1);
+  ser_->setRts(1);
+  time_->sleep(COMMAND_WAIT);
 }
 
 void roombaSci::startup(void)
 {
-	sendOPCODE(roombaSci::OC_START);
-	sendOPCODE(roombaSci::OC_CONTROL);
+  sendOPCODE(roombaSci::OC_START);
+  sendOPCODE(roombaSci::OC_CONTROL);
 }
 
-void roombaSci::powerOff(){
-	sendOPCODE(roombaSci::OC_POWER);
-	time_->sleep(COMMAND_WAIT);
+void roombaSci::powerOff()
+{
+  sendOPCODE(roombaSci::OC_POWER);
+  time_->sleep(COMMAND_WAIT);
 }
 
-void roombaSci::clean(){
-	sendOPCODE(roombaSci::OC_CLEAN);
-	time_->sleep(COMMAND_WAIT);
+void roombaSci::clean()
+{
+  sendOPCODE(roombaSci::OC_CLEAN);
+  time_->sleep(COMMAND_WAIT);
 }
 
-void roombaSci::safe(){
-	sendOPCODE(roombaSci::OC_SAFE);
-	time_->sleep(COMMAND_WAIT);
+void roombaSci::safe()
+{
+  sendOPCODE(roombaSci::OC_SAFE);
+  time_->sleep(COMMAND_WAIT);
 }
-void roombaSci::full(){
-	sendOPCODE(roombaSci::OC_FULL);
-	time_->sleep(COMMAND_WAIT);
+void roombaSci::full()
+{
+  sendOPCODE(roombaSci::OC_FULL);
+  time_->sleep(COMMAND_WAIT);
 }
-void roombaSci::spot(){
-	sendOPCODE(roombaSci::OC_SPOT);
-	time_->sleep(COMMAND_WAIT);
+void roombaSci::spot()
+{
+  sendOPCODE(roombaSci::OC_SPOT);
+  time_->sleep(COMMAND_WAIT);
 }
-void roombaSci::max(){
-	sendOPCODE(roombaSci::OC_MAX);
-	time_->sleep(COMMAND_WAIT);
+void roombaSci::max()
+{
+  sendOPCODE(roombaSci::OC_MAX);
+  time_->sleep(COMMAND_WAIT);
 }
 
-void roombaSci::dock(){
-	const unsigned char seq[]={OC_BUTTONS, roombaSci::BUTTON_DOCK};
-	ser_->write(seq,2);
-	time_->sleep(COMMAND_WAIT);
+void roombaSci::dock()
+{
+  const unsigned char seq[] = {OC_BUTTONS, roombaSci::BUTTON_DOCK};
+  ser_->write(seq, 2);
+  time_->sleep(COMMAND_WAIT);
 }
 
 // example
 // MB_MAIN_BRUSH | MB_VACUUM | MB_SIDE_BRUSH
 // puts all motors driving.
-void roombaSci::driveMotors(roombaSci::MOTOR_BITS state){
-	const unsigned char seq[]={OC_MOTORS, state};
-	ser_->write(seq,2);
-	time_->sleep(COMMAND_WAIT);
+void roombaSci::driveMotors(roombaSci::MOTOR_BITS state)
+{
+  const unsigned char seq[] = {OC_MOTORS, state};
+  ser_->write(seq, 2);
+  time_->sleep(COMMAND_WAIT);
 }
 
-void roombaSci::forceSeekingDock(){
-	const unsigned char seq[]={OC_FORCE_SEEKING_DOCK};
-	ser_->write(seq,1);
-	time_->sleep(COMMAND_WAIT);
+void roombaSci::forceSeekingDock()
+{
+  const unsigned char seq[] = {OC_FORCE_SEEKING_DOCK};
+  ser_->write(seq, 1);
+  time_->sleep(COMMAND_WAIT);
 }
 
-
-
-void roombaSci::drive(short velocity, short radius){
-
-	unsigned char vhi = (unsigned char)(velocity >> 8);
-	unsigned char vlo = (unsigned char)(velocity & 0xff);
-	unsigned char rhi = (unsigned char)(radius   >> 8);
-	unsigned char rlo = (unsigned char)(radius   & 0xff);
-
-	const unsigned char seq[]={OC_DRIVE, vhi, vlo, rhi, rlo};
-	ser_->write(seq,5);
-	time_->sleep(COMMAND_WAIT);
+void roombaSci::drive(int16_t velocity, int16_t radius)
+{
+  unsigned char vhi = (unsigned char)(velocity >> 8);
+  unsigned char vlo = (unsigned char)(velocity & 0xff);
+  unsigned char rhi = (unsigned char)(radius >> 8);
+  unsigned char rlo = (unsigned char)(radius & 0xff);
+
+  const unsigned char seq[] = {OC_DRIVE, vhi, vlo, rhi, rlo};
+  ser_->write(seq, 5);
+  time_->sleep(COMMAND_WAIT);
 }
 
-void roombaSci::driveDirect(float velocity, float yawrate){
-	short right=1000*(velocity+0.5*0.235*yawrate);
-	short left=1000*(velocity-0.5*0.235*yawrate);
+void roombaSci::driveDirect(float velocity, float yawrate)
+{
+  const int16_t right = 1000 * (velocity + 0.5 * 0.235 * yawrate);
+  const int16_t left = 1000 * (velocity - 0.5 * 0.235 * yawrate);
 
-	unsigned char rhi =  (unsigned char)(right >> 8);
-	unsigned char  rlo = (unsigned char)(right & 0xff);
-	unsigned char  lhi = (unsigned char)(left  >> 8);
-	unsigned char  llo = (unsigned char)(left  & 0xff);
+  unsigned char rhi = (unsigned char)(right >> 8);
+  unsigned char rlo = (unsigned char)(right & 0xff);
+  unsigned char lhi = (unsigned char)(left >> 8);
+  unsigned char llo = (unsigned char)(left & 0xff);
 
-	const unsigned char seq[]={OC_DRIVE_DIRECT, rhi, rlo, lhi, llo};
-	ser_->write(seq,5);
-	time_->sleep(COMMAND_WAIT);
+  const unsigned char seq[] = {OC_DRIVE_DIRECT, rhi, rlo, lhi, llo};
+  ser_->write(seq, 5);
+  time_->sleep(COMMAND_WAIT);
 }
 
-void roombaSci::drivePWM(int right_pwm, int left_pwm){
-	short right=255.0/100.0*right_pwm;
-	short left=255.0/100.0*left_pwm;
-
-	ROS_INFO("right = %d,left = %d",right,left);
-	unsigned char rhi =  (unsigned char)(right >> 8);
-	unsigned char rlo = (unsigned char)(right & 0xff);
-	unsigned char lhi = (unsigned char)(left >> 8);
-	unsigned char llo = (unsigned char)(left & 0xff);
-
-	const unsigned char seq[]={OC_DRIVE_PWM, rhi, rlo, lhi, llo};
-	ser_->write(seq,5);
-	time_->sleep(COMMAND_WAIT);
+void roombaSci::drivePWM(int right_pwm, int left_pwm)
+{
+  int16_t right = 255.0 / 100.0 * right_pwm;
+  int16_t left = 255.0 / 100.0 * left_pwm;
+
+  ROS_INFO("right = %d,left = %d", right, left);
+  unsigned char rhi = (unsigned char)(right >> 8);
+  unsigned char rlo = (unsigned char)(right & 0xff);
+  unsigned char lhi = (unsigned char)(left >> 8);
+  unsigned char llo = (unsigned char)(left & 0xff);
+
+  const unsigned char seq[] = {OC_DRIVE_PWM, rhi, rlo, lhi, llo};
+  ser_->write(seq, 5);
+  time_->sleep(COMMAND_WAIT);
 }
 
-float roombaSci::velToPWM(float velocity){
-	float pwm;
-	if(velocity>0){
-		pwm=173.38*velocity+14.55;		//ff para
-	}else if(velocity<0){
-		pwm=206*velocity-13.25;		//ff para
-	}else{
-		pwm=0;
-	}
-	return pwm;
+float roombaSci::velToPWM(float velocity)
+{
+  float pwm;
+  if (velocity > 0)
+  {
+    pwm = 173.38 * velocity + 14.55;  // ff para
+  }
+  else if (velocity < 0)
+  {
+    pwm = 206 * velocity - 13.25;  // ff para
+  }
+  else
+  {
+    pwm = 0;
+  }
+  return pwm;
 }
 
-void roombaSci::song(unsigned char song_number, unsigned char song_length){
-	const unsigned char mode_seq[]={OC_SAFE};
+void roombaSci::song(unsigned char song_number, unsigned char song_length)
+{
+  const unsigned char mode_seq[] = {OC_SAFE};
 
-	ser_->write(mode_seq,1);
-	time_->sleep(COMMAND_WAIT);
+  ser_->write(mode_seq, 1);
+  time_->sleep(COMMAND_WAIT);
 
-	const unsigned char command_seq[]={OC_SONG, song_number, song_length, 60, 126};
+  const unsigned char command_seq[] = {OC_SONG, song_number, song_length, 60, 126};
 
-	ser_->write(command_seq,2*song_length+3);
-	time_->sleep(COMMAND_WAIT);
+  ser_->write(command_seq, 2 * song_length + 3);
+  time_->sleep(COMMAND_WAIT);
 }
 
-void roombaSci::playing(unsigned char song_number){
-	const unsigned char command_seq[]={OC_PLAY, song_number};
+void roombaSci::playing(unsigned char song_number)
+{
+  const unsigned char command_seq[] = {OC_PLAY, song_number};
 
-	ser_->write(command_seq,2);
-	time_->sleep(COMMAND_WAIT);
+  ser_->write(command_seq, 2);
+  time_->sleep(COMMAND_WAIT);
 }
 
-
 int roombaSci::sendOPCODE(roombaSci::OPCODE oc)
 {
-	const unsigned char uc = (unsigned char)oc;
-	int ret = ser_->write(&uc,1);
-	time_->sleep(COMMAND_WAIT);
-	return ret;
+  const unsigned char uc = (unsigned char)oc;
+  int ret = ser_->write(&uc, 1);
+  time_->sleep(COMMAND_WAIT);
+  return ret;
 }
 
 int roombaSci::receive(void)
 {
-	return ser_->read(packet_,80);
+  return ser_->read(packet_, 80);
 }
 
 int roombaSci::receive(unsigned char* pack, int byte)
 {
-	return ser_->read(pack,byte);
+  return ser_->read(pack, byte);
 }
 
-int roombaSci::getSensors(roomba_500driver_meiji::Roomba500State& sensor){
-	const unsigned char seq[]={OC_SENSORS, ALL_PACKET};
-	int ret = ser_->write(seq,2);
-	time_->sleep(COMMAND_WAIT);
+int roombaSci::getSensors(roomba_500driver_meiji::Roomba500State& sensor)
+{
+  const unsigned char seq[] = {OC_SENSORS, ALL_PACKET};
+  int ret = ser_->write(seq, 2);
+  time_->sleep(COMMAND_WAIT);
 
-	int nbyte;
-	nbyte=receive();
+  int nbyte;
+  nbyte = receive();
 
-	if(nbyte==80){
-		packetToStruct(sensor, packet_);
-	}
+  if (nbyte == 80)
+  {
+    packetToStruct(sensor, packet_);
+  }
 
-	time_->sleep(COMMAND_WAIT);
+  time_->sleep(COMMAND_WAIT);
 
-	return ret;
+  return ret;
 }
 
-
 void roombaSci::packetToStruct(
-	roomba_500driver_meiji::Roomba500State& ret,
-	const unsigned char* pack
-){
-
-	ret.bump.right=(bool)(0x01&pack[0]);
-	ret.bump.left=(bool)(0x01&(pack[0]>>1));
-
-	ret.wheeldrop.right=(bool)(0x01&(pack[0]>>2));
-	ret.wheeldrop.left=(bool)(0x01&(pack[0]>>3));
-	ret.wheeldrop.caster=(bool)(0x01&(pack[0]>>4));
-
-
-	ret.wall=(bool)(0x01&(pack[1]));
-	ret.wall_signal=(unsigned short)((pack[26]<<8)|pack[27]);
-
-	ret.cliff.left=(bool)(0x01&(pack[2]));
-	ret.cliff.front_left=(bool)(0x01&(pack[3]));
-	ret.cliff.right=(bool)(0x01&(pack[4]));
-	ret.cliff.front_right=(bool)(0x01&(pack[5]));
-
-	ret.dirt_detect=(unsigned short)(pack[8]);
-
-	ret.cliff.left_signal=(unsigned short)((pack[28]<<8)|pack[29]);
-	ret.cliff.front_left_signal=(unsigned short)((pack[30]<<8)|pack[31]);
-	ret.cliff.front_right_signal=(unsigned short)((pack[32]<<8)|pack[33]);
-	ret.cliff.right_signal=(unsigned short)((pack[34]<<8)|pack[35]);
-
-	ret.virtual_wall=(bool)(0x01&(pack[6]));
-
-	ret.motor_overcurrents.side_brush=(bool)(0x01&(pack[7]));
-	ret.motor_overcurrents.vacuum=(bool)(0x01&(pack[7]>>1));
-	ret.motor_overcurrents.main_brush=(bool)(0x01&(pack[7]>>2));
-	ret.motor_overcurrents.drive_right=(bool)(0x01&(pack[7]>>3));
-	ret.motor_overcurrents.drive_left=(bool)(0x01&(pack[7]>>4));
-
-
-	ret.dirt_detector.left=(pack[8]);
-	ret.dirt_detector.right=(pack[9]);
-
-	ret.remote_control_command=(pack[10]);
-
-	ret.buttons.max=(bool)(0x01&(pack[11]));
-	ret.buttons.clean=(bool)(0x01&(pack[11]>>1));
-	ret.buttons.spot=(bool)(0x01&(pack[11]>>2));
-	ret.buttons.power=(bool)(0x01&(pack[11]>>3));
-
-	ret.distance=(short)((pack[12]<<8)|pack[13]);
-	ret.angle=(short)((pack[14]<<8)|pack[15]);
-	ret.requested_wheel_velocity.right=(short)((pack[48]<<8)|pack[49]);
-	ret.requested_wheel_velocity.left=(short)((pack[50]<<8)|pack[51]);
-
-	ret.dirt_detect=(unsigned short)(pack[39]);
-	ret.open_interface_mode=(unsigned short)(pack[40]);
-
-	ret.song.number=(unsigned short)(pack[41]);
-	ret.song.playing=(unsigned short)(pack[42]);
-
-	ret.oi_stream_num_packets=(unsigned short)(pack[43]);
-
-	ret.requested_velocity=(short)((pack[44]<<8)|pack[45]);
-	ret.requested_radius=(short)((pack[46]<<8)|pack[47]);
-	ret.encoder_counts.left=(unsigned short)((pack[52]<<8)|pack[53]);
-	ret.encoder_counts.right=(unsigned short)((pack[54]<<8)|pack[55]);
-
-	ret.light_bumper.bumper=(unsigned short)(pack[56]);
-	ret.light_bumper.left=(unsigned short)((pack[57]<<8)|pack[58]);;
-	ret.light_bumper.front_left=(unsigned short)((pack[59]<<8)|pack[60]);
-	ret.light_bumper.center_left=(unsigned short)((pack[61]<<8)|pack[62]);
-	ret.light_bumper.center_right=(unsigned short)((pack[63]<<8)|pack[64]);
-	ret.light_bumper.front_right=(unsigned short)((pack[65]<<8)|pack[66]);
-	ret.light_bumper.right=(unsigned short)((pack[67]<<8)|pack[68]);
-
-	ret.opcode.left=(unsigned short)(pack[69]);
-	ret.opcode.right=(unsigned short)(pack[70]);
-
-	ret.stasis=(bool)(0x01&(pack[79]));
-
-	if(std::abs((int)ret.encoder_counts.right-(int)enc_count_r_) >= 60000){
-		if(ret.encoder_counts.right > enc_count_r_){
-			d_enc_count_r_=-65535-enc_count_r_+ret.encoder_counts.right;
-		}else{
-			d_enc_count_r_=65535-enc_count_r_+ret.encoder_counts.right;
-		}
-	}else{
-		d_enc_count_r_=ret.encoder_counts.right - enc_count_r_;
-	}
-
-	if(std::abs((int)ret.encoder_counts.left-(int)enc_count_l_) >= 60000){
-		if(ret.encoder_counts.left > enc_count_l_){
-			d_enc_count_l_=-65535-enc_count_l_+ret.encoder_counts.left;
-		}else{
-			d_enc_count_l_=65535-enc_count_l_+ret.encoder_counts.left;
-		}
-	}else{
-		d_enc_count_l_=ret.encoder_counts.left - enc_count_l_;
-	}
-
-	enc_count_r_ = ret.encoder_counts.right;
-	enc_count_l_ =  ret.encoder_counts.left;
-
-	ret.charging_state=pack[16];
-	ret.voltage=((pack[17]<<8)|pack[18]);
-	ret.current=((pack[19]<<8)|pack[20]);
-	ret.temperature=pack[21];
-	ret.charge=((pack[22]<<8)|pack[23]);
-	ret.capacity=((pack[24]<<8)|pack[25]);
-
+    roomba_500driver_meiji::Roomba500State& ret,
+    const unsigned char* pack)
+{
+  ret.bump.right = static_cast<bool>(0x01 & pack[0]);
+  ret.bump.left = static_cast<bool>(0x01 & (pack[0] >> 1));
+
+  ret.wheeldrop.right = static_cast<bool>(0x01 & (pack[0] >> 2));
+  ret.wheeldrop.left = static_cast<bool>(0x01 & (pack[0] >> 3));
+  ret.wheeldrop.caster = static_cast<bool>(0x01 & (pack[0] >> 4));
+
+  ret.wall = static_cast<bool>(0x01 & (pack[1]));
+  ret.wall_signal = (uint16_t)((pack[26] << 8) | pack[27]);
+
+  ret.cliff.left = static_cast<bool>(0x01 & (pack[2]));
+  ret.cliff.front_left = static_cast<bool>(0x01 & (pack[3]));
+  ret.cliff.right = static_cast<bool>(0x01 & (pack[4]));
+  ret.cliff.front_right = static_cast<bool>(0x01 & (pack[5]));
+
+  ret.dirt_detect = (uint16_t)(pack[8]);
+
+  ret.cliff.left_signal = (uint16_t)((pack[28] << 8) | pack[29]);
+  ret.cliff.front_left_signal = (uint16_t)((pack[30] << 8) | pack[31]);
+  ret.cliff.front_right_signal = (uint16_t)((pack[32] << 8) | pack[33]);
+  ret.cliff.right_signal = (uint16_t)((pack[34] << 8) | pack[35]);
+
+  ret.virtual_wall = static_cast<bool>(0x01 & (pack[6]));
+
+  ret.motor_overcurrents.side_brush = static_cast<bool>(0x01 & (pack[7]));
+  ret.motor_overcurrents.vacuum = static_cast<bool>(0x01 & (pack[7] >> 1));
+  ret.motor_overcurrents.main_brush = static_cast<bool>(0x01 & (pack[7] >> 2));
+  ret.motor_overcurrents.drive_right = static_cast<bool>(0x01 & (pack[7] >> 3));
+  ret.motor_overcurrents.drive_left = static_cast<bool>(0x01 & (pack[7] >> 4));
+
+  ret.dirt_detector.left = (pack[8]);
+  ret.dirt_detector.right = (pack[9]);
+
+  ret.remote_control_command = (pack[10]);
+
+  ret.buttons.max = static_cast<bool>(0x01 & (pack[11]));
+  ret.buttons.clean = static_cast<bool>(0x01 & (pack[11] >> 1));
+  ret.buttons.spot = static_cast<bool>(0x01 & (pack[11] >> 2));
+  ret.buttons.power = static_cast<bool>(0x01 & (pack[11] >> 3));
+
+  ret.distance = (int16_t)((pack[12] << 8) | pack[13]);
+  ret.angle = (int16_t)((pack[14] << 8) | pack[15]);
+  ret.requested_wheel_velocity.right = (int16_t)((pack[48] << 8) | pack[49]);
+  ret.requested_wheel_velocity.left = (int16_t)((pack[50] << 8) | pack[51]);
+
+  ret.dirt_detect = (uint16_t)(pack[39]);
+  ret.open_interface_mode = (uint16_t)(pack[40]);
+
+  ret.song.number = (uint16_t)(pack[41]);
+  ret.song.playing = (uint16_t)(pack[42]);
+
+  ret.oi_stream_num_packets = (uint16_t)(pack[43]);
+
+  ret.requested_velocity = (int16_t)((pack[44] << 8) | pack[45]);
+  ret.requested_radius = (int16_t)((pack[46] << 8) | pack[47]);
+  ret.encoder_counts.left = (uint16_t)((pack[52] << 8) | pack[53]);
+  ret.encoder_counts.right = (uint16_t)((pack[54] << 8) | pack[55]);
+
+  ret.light_bumper.bumper = (uint16_t)(pack[56]);
+  ret.light_bumper.left = (uint16_t)((pack[57] << 8) | pack[58]);
+
+  ret.light_bumper.front_left = (uint16_t)((pack[59] << 8) | pack[60]);
+  ret.light_bumper.center_left = (uint16_t)((pack[61] << 8) | pack[62]);
+  ret.light_bumper.center_right = (uint16_t)((pack[63] << 8) | pack[64]);
+  ret.light_bumper.front_right = (uint16_t)((pack[65] << 8) | pack[66]);
+  ret.light_bumper.right = (uint16_t)((pack[67] << 8) | pack[68]);
+
+  ret.opcode.left = (uint16_t)(pack[69]);
+  ret.opcode.right = (uint16_t)(pack[70]);
+
+  ret.stasis = static_cast<bool>(0x01 & (pack[79]));
+
+  if (std::abs(static_cast<int>(ret.encoder_counts.right) - static_cast<int>(enc_count_r_)) >= 60000)
+  {
+    if (ret.encoder_counts.right > enc_count_r_)
+    {
+      d_enc_count_r_ = -65535 - enc_count_r_ + ret.encoder_counts.right;
+    }
+    else
+    {
+      d_enc_count_r_ = 65535 - enc_count_r_ + ret.encoder_counts.right;
+    }
+  }
+  else
+  {
+    d_enc_count_r_ = ret.encoder_counts.right - enc_count_r_;
+  }
+
+  if (std::abs(static_cast<int>(ret.encoder_counts.left) - static_cast<int>(enc_count_l_)) >= 60000)
+  {
+    if (ret.encoder_counts.left > enc_count_l_)
+    {
+      d_enc_count_l_ = -65535 - enc_count_l_ + ret.encoder_counts.left;
+    }
+    else
+    {
+      d_enc_count_l_ = 65535 - enc_count_l_ + ret.encoder_counts.left;
+    }
+  }
+  else
+  {
+    d_enc_count_l_ = ret.encoder_counts.left - enc_count_l_;
+  }
+
+  enc_count_r_ = ret.encoder_counts.right;
+  enc_count_l_ = ret.encoder_counts.left;
+
+  ret.charging_state = pack[16];
+  ret.voltage = ((pack[17] << 8) | pack[18]);
+  ret.current = ((pack[19] << 8) | pack[20]);
+  ret.temperature = pack[21];
+  ret.charge = ((pack[22] << 8) | pack[23]);
+  ret.capacity = ((pack[24] << 8) | pack[25]);
 }
 
-
 #ifdef roombaSci_TEST
 
 #include <stdio.h>
 int main(void)
 {
-	roombaSci roomba(B19200,"/dev/ttyUSB0");
+  roombaSci roomba(B19200, "/dev/ttyUSB0");
 
-	puts("opcode wakeup");
-	roomba.wakeup();
+  puts("opcode wakeup");
+  roomba.wakeup();
 
-	puts("opcode start and control");
-	roomba.startup();
+  puts("opcode start and control");
+  roomba.startup();
 
-	puts("opcode drive");
-	roomba.drive(100,STRAIGHT_RADIUS);
+  puts("opcode drive");
+  roomba.drive(100, STRAIGHT_RADIUS);
 
-	for(int i=0; i<50; i++){
-		roomba.getSensors();
-		roomba.time_->sleep(0.1);
-	}
+  for (int i = 0; i < 50; i++)
+  {
+    roomba.getSensors();
+    roomba.time_->sleep(0.1);
+  }
 
-	puts("opcode poweroff");
-	roomba.powerOff();
+  puts("opcode poweroff");
+  roomba.powerOff();
 }
 
-#endif	// DEBUG
+#endif  // DEBUG
diff --git a/roomba_500driver_meiji/src/serial.cpp b/roomba_500driver_meiji/src/serial.cpp
old mode 100755
new mode 100644
index fcbca54..d265459
--- a/roomba_500driver_meiji/src/serial.cpp
+++ b/roomba_500driver_meiji/src/serial.cpp
@@ -29,114 +29,110 @@
 #include "roomba_500driver_meiji/serial.h"
 
 #include <iostream>
-using namespace std;
 
-//#define DEBUG
+// #define DEBUG
 
 Serial::Serial(int baudrate, const char* modemdevice, int vmin, int lflag)
 {
-#if 1 
-		struct termios toptions;
-
-		fd_ = open(modemdevice, O_RDWR | O_NOCTTY | O_NDELAY );
-		if (fd_ == -1)  {     // Could not open the port.
-			perror("roomba_init_serialport: Unable to open port ");
-			exit(-1);
-    }
-    
-		tcgetattr(fd_, &oldtio_);
-		if (tcgetattr(fd_, &toptions) < 0) {
-			perror("roomba_init_serialport: Couldn't get term attributes");
-				exit(-1);
-		}
-
-		cfsetispeed(&toptions, baudrate);
-		cfsetospeed(&toptions, baudrate);
-
-		// 8N1
-		toptions.c_cflag &= ~PARENB;
-		toptions.c_cflag &= ~CSTOPB;
-		toptions.c_cflag &= ~CSIZE;
-		toptions.c_cflag |= CS8;
-		// no flow control
-		toptions.c_cflag &= ~CRTSCTS;
-
-		toptions.c_cflag    |= CREAD | CLOCAL;  // turn on READ & ignore ctrl lines
-		toptions.c_iflag    &= ~(IXON | IXOFF | IXANY); // turn off s/w flow ctrl
-
-		toptions.c_lflag    &= ~(ICANON | ECHO | ECHOE | ISIG); // make raw
-		toptions.c_oflag    &= ~OPOST; // make raw
-
-		toptions.c_cc[VMIN]  = 26;
-		toptions.c_cc[VTIME] = 2;           // FIXME: not sure about this
-
-		if( tcsetattr(fd_, TCSANOW, &toptions) < 0) {
-			perror("roomba_init_serialport: Couldn't set term attributes");
-			exit(-1);
-		}
+#if 1
+  struct termios toptions;
+
+  fd_ = open(modemdevice, O_RDWR | O_NOCTTY | O_NDELAY);
+  if (fd_ == -1)
+  {  // Could not open the port.
+    perror("roomba_init_serialport: Unable to open port ");
+    exit(-1);
+  }
+
+  tcgetattr(fd_, &oldtio_);
+  if (tcgetattr(fd_, &toptions) < 0)
+  {
+    perror("roomba_init_serialport: Couldn't get term attributes");
+    exit(-1);
+  }
+
+  cfsetispeed(&toptions, baudrate);
+  cfsetospeed(&toptions, baudrate);
+
+  // 8N1
+  toptions.c_cflag &= ~PARENB;
+  toptions.c_cflag &= ~CSTOPB;
+  toptions.c_cflag &= ~CSIZE;
+  toptions.c_cflag |= CS8;
+  // no flow control
+  toptions.c_cflag &= ~CRTSCTS;
+
+  toptions.c_cflag |= CREAD | CLOCAL;           // turn on READ & ignore ctrl lines
+  toptions.c_iflag &= ~(IXON | IXOFF | IXANY);  // turn off s/w flow ctrl
+
+  toptions.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);  // make raw
+  toptions.c_oflag &= ~OPOST;                           // make raw
+
+  toptions.c_cc[VMIN] = 26;
+  toptions.c_cc[VTIME] = 2;  // FIXME: not sure about this
+
+  if (tcsetattr(fd_, TCSANOW, &toptions) < 0)
+  {
+    perror("roomba_init_serialport: Couldn't set term attributes");
+    exit(-1);
+  }
 #endif
-
 }
 
-
 Serial::~Serial()
 {
-	tcsetattr(fd_,TCSANOW,&oldtio_);
-	close(fd_);
+  tcsetattr(fd_, TCSANOW, &oldtio_);
+  close(fd_);
 }
 
-
 int Serial::read(unsigned char* p, int len)
 {
-	return ::read(fd_, p, len);
+  return ::read(fd_, p, len);
 }
 
 int Serial::write(const unsigned char* p, int len)
 {
-	return ::write(fd_, p, len);
+  return ::write(fd_, p, len);
 }
 
-void Serial::setVmin(int vmin) {
- 
-	newtio_.c_iflag = IGNPAR;
-
-	newtio_.c_cc[VTIME]    = 1;   /* キャラクタ間タイマ未使用*/
-	newtio_.c_cc[VMIN]     = vmin;   /* vmin文字受け取るまでブロックする*/
+void Serial::setVmin(int vmin)
+{
+  newtio_.c_iflag = IGNPAR;
 
+  newtio_.c_cc[VTIME] = 1;   /* キャラクタ間タイマ未使用*/
+  newtio_.c_cc[VMIN] = vmin; /* vmin文字受け取るまでブロックする*/
 }
 
 void Serial::setRts(int sw)
 {
-	int status;
-	ioctl(fd_, TIOCMGET, &status); /* set the serial port status */
+  int status;
+  ioctl(fd_, TIOCMGET, &status); /* set the serial port status */
 
-	if(sw)      /* set the RTS line */
-		status &= ~TIOCM_RTS;
-	else
-		status |= TIOCM_RTS;
+  if (sw) /* set the RTS line */
+    status &= ~TIOCM_RTS;
+  else
+    status |= TIOCM_RTS;
 
-	ioctl(fd_, TIOCMSET, &status); /* set the serial port status */
+  ioctl(fd_, TIOCMSET, &status); /* set the serial port status */
 }
 
 #ifdef DEBUG
 int main()
 {
-	char rdata[255];
-	char sdata[255];
+  char rdata[255];
+  char sdata[255];
 
+  Serial test(B19200, "/dev/ttyUSB0");
 
-	Serial test(B19200, "/dev/ttyUSB0");
+  snprintf(sdata, siziof(sdata), "V\r");
 
-	sprintf(sdata, "V\r");
-	
-	test.write_serial(sdata, strlen(sdata));
+  test.write_serial(sdata, strlen(sdata));
 
-	test.read_serial(rdata);
+  test.read_serial(rdata);
 
-	printf("%s",rdata);
+  printf("%s", rdata);
 
-	return 0;
+  return 0;
 }
 
-
 #endif
diff --git a/roomba_500driver_meiji/src/twist_to_roombactrl_converter.cpp b/roomba_500driver_meiji/src/twist_to_roombactrl_converter.cpp
index 8526c7b..3fc0516 100644
--- a/roomba_500driver_meiji/src/twist_to_roombactrl_converter.cpp
+++ b/roomba_500driver_meiji/src/twist_to_roombactrl_converter.cpp
@@ -1,3 +1,5 @@
+// Copyright 2007-2023 amsl
+
 #include <ros/ros.h>
 #include <geometry_msgs/Twist.h>
 #include <roomba_500driver_meiji/RoombaCtrl.h>
@@ -7,39 +9,43 @@ namespace roomba_500driver_meiji
 class TwistToRoombactrlConverter
 {
 public:
-    TwistToRoombactrlConverter(void)
-    {
-        ctrl_pub_= nh_.advertise<roomba_500driver_meiji::RoombaCtrl>("roomba/control", 1);
-        cmd_vel_sub_ = nh_.subscribe("cmd_vel", 1,
-             &TwistToRoombactrlConverter::cmd_vel_callback, this, ros::TransportHints().reliable().tcpNoDelay(true));
-    }
+  TwistToRoombactrlConverter(void)
+  {
+    ctrl_pub_ = nh_.advertise<roomba_500driver_meiji::RoombaCtrl>("roomba/control", 1);
+    cmd_vel_sub_ = nh_.subscribe("cmd_vel",
+                                 1,
+                                 &TwistToRoombactrlConverter::cmd_vel_callback,
+                                 this,
+                                 ros::TransportHints().reliable().tcpNoDelay(true));
+  }
+
+  void cmd_vel_callback(const geometry_msgs::TwistConstPtr& msg)
+  {
+    roomba_500driver_meiji::RoombaCtrl control;
+    control.mode = roomba_500driver_meiji::RoombaCtrl::DRIVE_DIRECT;
+    control.cntl.linear.x = msg->linear.x;
+    control.cntl.angular.z = msg->angular.z;
+    ctrl_pub_.publish(control);
+  }
 
-    void cmd_vel_callback(const geometry_msgs::TwistConstPtr& msg)
-    {
-        roomba_500driver_meiji::RoombaCtrl control;
-        control.mode = roomba_500driver_meiji::RoombaCtrl::DRIVE_DIRECT;
-        control.cntl.linear.x = msg->linear.x;
-        control.cntl.angular.z = msg->angular.z;
-        ctrl_pub_.publish(control);
-    }
+  void process(void)
+  {
+    ros::spin();
+  }
 
-    void process(void)
-    {
-        ros::spin();
-    }
 private:
-    ros::NodeHandle nh_;
-    ros::NodeHandle local_nh_;
-    ros::Publisher ctrl_pub_;
-    ros::Subscriber cmd_vel_sub_;
+  ros::NodeHandle nh_;
+  ros::NodeHandle local_nh_;
+  ros::Publisher ctrl_pub_;
+  ros::Subscriber cmd_vel_sub_;
 };
 
-}
+}  // namespace roomba_500driver_meiji
 
 int main(int argc, char** argv)
 {
-    ros::init(argc, argv, "twist_to_roombactrl_converter");
-    roomba_500driver_meiji::TwistToRoombactrlConverter trc;
-    trc.process();
-    return 0;
+  ros::init(argc, argv, "twist_to_roombactrl_converter");
+  roomba_500driver_meiji::TwistToRoombactrlConverter trc;
+  trc.process();
+  return 0;
 }
diff --git a/roomba_description/CMakeLists.txt b/roomba_description/CMakeLists.txt
index a576fa8..3c4245e 100644
--- a/roomba_description/CMakeLists.txt
+++ b/roomba_description/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
 project(roomba_description)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
@@ -182,11 +182,12 @@ include_directories(
 # )
 
 ## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
+install(FILES
+  config
+  urdf
+  launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
 
 #############
 ## Testing ##
diff --git a/roomba_teleop_meiji/CMakeLists.txt b/roomba_teleop_meiji/CMakeLists.txt
index b0decd2..f372b86 100755
--- a/roomba_teleop_meiji/CMakeLists.txt
+++ b/roomba_teleop_meiji/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(roomba_teleop_meiji)
 
 ## Find catkin macros and libraries
@@ -160,11 +160,11 @@ add_dependencies(electric_joystick_drive roomba_500driver_meiji_generate_message
 # )
 
 ## Mark executables and/or libraries for installation
-# install(TARGETS roomba_teleop_meiji roomba_teleop_meiji_node
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
+install(TARGETS electric_joystick_drive
+  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}/
@@ -192,3 +192,10 @@ add_dependencies(electric_joystick_drive roomba_500driver_meiji_generate_message
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
+
+if(CATKIN_ENABLE_TESTING)
+  find_package(roslint REQUIRED)
+  set(ROSLINT_CPP_OPTS "--filter=-build/c++11,-runtime/references,-legal/copyright")
+  roslint_cpp()
+  roslint_add_test()
+endif()
diff --git a/roomba_teleop_meiji/package.xml b/roomba_teleop_meiji/package.xml
index f0b5902..142b320 100755
--- a/roomba_teleop_meiji/package.xml
+++ b/roomba_teleop_meiji/package.xml
@@ -4,7 +4,7 @@
   <version>0.0.0</version>
   <description>The roomba_teleop_meiji package</description>
 
-  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
   <maintainer email="amsl@todo.todo">amsl</maintainer>
@@ -48,6 +48,8 @@
   <run_depend>roomba_500driver_meiji</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>sensor_msgs</run_depend>
+  <test_depend>rostest</test_depend>
+  <test_depend>roslint</test_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
@@ -55,4 +57,4 @@
     <!-- Other tools can request additional information be placed here -->
 
   </export>
-</package>
\ No newline at end of file
+</package>
diff --git a/roomba_teleop_meiji/src/electric_joystick_drive.cpp b/roomba_teleop_meiji/src/electric_joystick_drive.cpp
old mode 100755
new mode 100644
index dd270b7..2f8c440
--- a/roomba_teleop_meiji/src/electric_joystick_drive.cpp
+++ b/roomba_teleop_meiji/src/electric_joystick_drive.cpp
@@ -17,7 +17,7 @@
  */
 //-----------------------------------------------------------------------------
 #include <ros/ros.h>
-//#include <joy/Joy.h>
+// #include <joy/Joy.h>
 #include <sensor_msgs/Joy.h>
 #include <roomba_500driver_meiji/RoombaCtrl.h>
 #include <boost/thread.hpp>
@@ -25,70 +25,88 @@ boost::mutex cntl_mutex_;
 boost::mutex std_mutex;
 
 #include <iostream>
+#include <string>
 #include <math.h>
 #include <time.h>
 #include <std_msgs/String.h>
-using namespace std;
 
-inline int toInt(string s){int v; istringstream sin(s);sin>>v;return v;}
-template<class T> inline string toString(T x) {ostringstream sout;sout<<x;return sout.str();}
+inline int toInt(std::string s)
+{
+  int v;
+  std::istringstream sin(s);
+  sin >> v;
+  return v;
+}
+template <class T>
+inline std::string toString(T x)
+{
+  std::ostringstream sout;
+  sout << x;
+  return sout.str();
+}
 
-//joy::Joy joy_in;
+// joy::Joy joy_in;
 sensor_msgs::Joy joy_in;
 ros::Publisher pub_state;
 
 bool flag = false;
-clock_t s,e;
+clock_t s, e;
 int l[10] = {-1};
 int c = 0;
 
 void stdCallback(const std_msgs::StringConstPtr& msg)
 {
-	boost::mutex::scoped_lock(std_mutex);
-	//cout<<"--"<<endl;
-	int a = toInt(msg->data);
-	int count = 0;
-	int run = 0;
-	
-	for(int i=0;i<10;i++) if(l[i] == a) count++;
-	if(count == 0 && a <= 10 && c <=10){
-		l[c] = a;
-		c++;
-	}
-	else run = 1;
-	
-	if(run ==0) flag = true;
+  boost::mutex::scoped_lock(std_mutex);
+  // std::cout<<"--"<<std::endl;
+  int a = toInt(msg->data);
+  int count = 0;
+  int run = 0;
+
+  for (int i = 0; i < 10; i++)
+    if (l[i] == a)
+      count++;
+  if (count == 0 && a <= 10 && c <= 10)
+  {
+    l[c] = a;
+    c++;
+  }
+  else
+    run = 1;
+
+  if (run == 0)
+    flag = true;
 }
 
-void joy_callback(const sensor_msgs::JoyConstPtr& msg){
-	boost::mutex::scoped_lock(cntl_mutex_);
-	//joy_in=*msg;
-	roomba_500driver_meiji::RoombaCtrl ctrl;
-	
-	ctrl.mode=roomba_500driver_meiji::RoombaCtrl::DRIVE;
-	/*
+void joy_callback(const sensor_msgs::JoyConstPtr& msg)
+{
+  boost::mutex::scoped_lock(cntl_mutex_);
+  // joy_in=*msg;
+  roomba_500driver_meiji::RoombaCtrl ctrl;
+
+  ctrl.mode = roomba_500driver_meiji::RoombaCtrl::DRIVE;
+  /*
 	if(msg->buttons[0]){
 		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::SAFE;
 		cout<<"SAFE mode"<<endl;
 	}
-	
+
 	if(msg->buttons[1]){
 		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::SPOT;
 		cout<<"SPOT mode"<<endl;
 	}
-	
+
 	if(msg->buttons[2]){
 		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::CLEAN;
 		cout<<"CLEAN mode"<<endl;
 	}
-	
+
 	if(msg->buttons[3]){
 		//ctrl.mode=roomba_500driver_meiji::RoombaCtrl::MAX;
 		//cout<<"MAX mode"<<endl;
 		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::DOCK;
 		cout<<"DOCK mode"<<endl;
 	}
-	
+
 	if(msg->buttons[4]){
 		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::MOTORS;
 		cout<<"MOTORS mode"<<endl;
@@ -105,7 +123,7 @@ void joy_callback(const sensor_msgs::JoyConstPtr& msg){
 		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::FULL;
 		cout<<"FULL mode"<<endl;
 	}
-	
+
 	if(msg->buttons[9]){	//for red controller
 	//if(msg->buttons[10]){
 		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::WAKEUP;
@@ -117,77 +135,77 @@ void joy_callback(const sensor_msgs::JoyConstPtr& msg){
 		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::POWER;
 		cout<<"POWER OFF"<<endl;
 	}*/
-	/*	
+  /*
 	ctrl.velocity=1.0*roomba_msgs::RoombaCtrl::DEFAULT_VELOCITY*msg->axes[1]+
 					roomba_msgs::RoombaCtrl::DEFAULT_VELOCITY*msg->axes[5];
-					
+
 	ctrl.radius=((float)ctrl.velocity/msg->axes[2])+0*roomba_msgs::RoombaCtrl::STRAIGHT_RADIUS;
-	
+
 	if(fabs(ctrl.radius)>roomba_msgs::RoombaCtrl::STRAIGHT_RADIUS){
 		ctrl.radius=roomba_msgs::RoombaCtrl::STRAIGHT_RADIUS;
 	}
-	
-	if(msg->axes[4]){ // 
+
+	if(msg->axes[4]){ //
 		ctrl.velocity=roomba_msgs::RoombaCtrl::DEFAULT_VELOCITY;
 		ctrl.radius=msg->axes[4];
 	}
 	*/
-	if(flag){
-		s = clock();
-		cout<<"STOP"<<endl;
-		clock_t sec = 0.0;
-		while(sec < 10.0){
-			ctrl.cntl.linear.x=0.0;
-			ctrl.cntl.angular.z=0.0;
-			ctrl.mode=roomba_500driver_meiji::RoombaCtrl::DRIVE_DIRECT;
-			pub_state.publish(ctrl);
-			e = clock();
-			sec = (double)(e-s)/CLOCKS_PER_SEC;
-		}
-		flag = false;
-		
-		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::SONG;
-		pub_state.publish(ctrl);
-
-		ctrl.cntl.linear.x=0.0;
-		ctrl.cntl.angular.z=0.0;
-		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::DRIVE_DIRECT;
-		cout<<ctrl.cntl.linear.x<<"   "<<ctrl.cntl.angular.z<<endl;
-	}
-	else if(msg->axes[1] || msg->axes[0]){ // 
-		ctrl.cntl.linear.x=0.005*roomba_500driver_meiji::RoombaCtrl::DEFAULT_VELOCITY*msg->axes[1];
-		ctrl.cntl.angular.z=(float)msg->axes[0];
-	
-		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::DRIVE_DIRECT;
-		//cout<<ctrl.velocity<<"   "<<ctrl.radius<<endl;
-		cout<<ctrl.cntl.linear.x<<"   "<<ctrl.cntl.angular.z<<endl;
-		//pub_state.publish(ctrl);
-	}	
-	
-	pub_state.publish(ctrl);
-}
+  if (flag)
+  {
+    s = clock();
+    std::cout << "STOP" << std::endl;
+    clock_t sec = 0.0;
+    while (sec < 10.0)
+    {
+      ctrl.cntl.linear.x = 0.0;
+      ctrl.cntl.angular.z = 0.0;
+      ctrl.mode = roomba_500driver_meiji::RoombaCtrl::DRIVE_DIRECT;
+      pub_state.publish(ctrl);
+      e = clock();
+      sec = static_cast<double>(e - s) / CLOCKS_PER_SEC;
+    }
+    flag = false;
 
+    ctrl.mode = roomba_500driver_meiji::RoombaCtrl::SONG;
+    pub_state.publish(ctrl);
 
+    ctrl.cntl.linear.x = 0.0;
+    ctrl.cntl.angular.z = 0.0;
+    ctrl.mode = roomba_500driver_meiji::RoombaCtrl::DRIVE_DIRECT;
+    std::cout << ctrl.cntl.linear.x << "   " << ctrl.cntl.angular.z << std::endl;
+  }
+  else if (msg->axes[1] || msg->axes[0])
+  {  //
+    ctrl.cntl.linear.x = 0.005 * roomba_500driver_meiji::RoombaCtrl::DEFAULT_VELOCITY * msg->axes[1];
+    ctrl.cntl.angular.z = static_cast<float>(msg->axes[0]);
 
-int main(int argc, char** argv) {
-	
-    ros::init(argc, argv, "joystick_drive");
-    ros::NodeHandle n;
+    ctrl.mode = roomba_500driver_meiji::RoombaCtrl::DRIVE_DIRECT;
+    // cout<<ctrl.velocity<<"   "<<ctrl.radius<<endl;
+    std::cout << ctrl.cntl.linear.x << "   " << ctrl.cntl.angular.z << std::endl;
+    // pub_state.publish(ctrl);
+  }
 
-	pub_state=n.advertise<roomba_500driver_meiji::RoombaCtrl>("/roomba/control", 100);
-	//ros::Publisher pub_odom= n.advertise<geometry_msgs::TransformStamped >("/global_frame", 100);
-	ros::Subscriber cntl_sub = n.subscribe("/joy", 100, joy_callback);
-	ros::Subscriber std_sub_ = n.subscribe("qrcode_data", 1, stdCallback);
-	/*
+  pub_state.publish(ctrl);
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "joystick_drive");
+  ros::NodeHandle n;
+
+  pub_state = n.advertise<roomba_500driver_meiji::RoombaCtrl>("/roomba/control", 100);
+  // ros::Publisher pub_odom= n.advertise<geometry_msgs::TransformStamped >("/global_frame", 100);
+  ros::Subscriber cntl_sub = n.subscribe("/joy", 100, joy_callback);
+  ros::Subscriber std_sub_ = n.subscribe("qrcode_data", 1, stdCallback);
+  /*
 	ros::Rate loop_rate(20);
 	while (ros::ok()) {
-		
+
 		ros::spinOnce();
         loop_rate.sleep();
     }
 	*/
-	
-	ros::spin();
-    return 0;
-}
 
+  ros::spin();
+  return 0;
+}
diff --git a/roomba_teleop_meiji/src/joystick_drive.cpp b/roomba_teleop_meiji/src/joystick_drive.cpp
old mode 100755
new mode 100644
index 76c3e56..d019c2f
--- a/roomba_teleop_meiji/src/joystick_drive.cpp
+++ b/roomba_teleop_meiji/src/joystick_drive.cpp
@@ -18,125 +18,132 @@
 #include <ros/ros.h>
 
 #include <joy/Joy.h>
-//#include <sensor_msgs/Joy.h>
+// #include <sensor_msgs/Joy.h>
 
 #include <roomba_500driver_meiji/RoombaCtrl.h>
 
-
 #include <boost/thread.hpp>
 boost::mutex cntl_mutex_;
 
 #include <iostream>
 #include <math.h>
-using namespace std;
 
 joy::Joy joy_in;
 ros::Publisher pub_state;
 
-void joy_callback(const joy::JoyConstPtr& msg){
-	boost::mutex::scoped_lock(cntl_mutex_);
-	//joy_in=*msg;
-	roomba_500driver_meiji::RoombaCtrl ctrl;
-	
-	ctrl.mode=roomba_500driver_meiji::RoombaCtrl::DRIVE;
-	
-	if(msg->buttons[0]){
-		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::SAFE;
-		cout<<"SAFE mode"<<endl;
-	}
-	
-	if(msg->buttons[1]){
-		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::SPOT;
-		cout<<"SPOT mode"<<endl;
-	}
-	
-	if(msg->buttons[2]){
-		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::CLEAN;
-		cout<<"CLEAN mode"<<endl;
-	}
-	
-	if(msg->buttons[3]){
-		//ctrl.mode=roomba_500driver_meiji::RoombaCtrl::MAX;
-		//cout<<"MAX mode"<<endl;
-		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::DOCK;
-		cout<<"DOCK mode"<<endl;
-	}
-	
-	if(msg->buttons[4]){
-		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::MOTORS;
-		cout<<"MOTORS mode"<<endl;
-	}
-	if(msg->buttons[6]){
-		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::MOTORS_OFF;
-		cout<<"MOTORS OFF mode"<<endl;
-	}
-	if(msg->buttons[5]){
-		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::FORCE_SEEK_DOCK;
-		cout<<"FORCE_SEEK_DOCK mode"<<endl;
-	}
-	if(msg->buttons[7]){
-		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::FULL;
-		cout<<"FULL mode"<<endl;
-	}
-	
-	if(msg->buttons[9]){	//for red controller
-	//if(msg->buttons[10]){
-		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::WAKEUP;
-		cout<<"WAKEUP"<<endl;
-	}
+void joy_callback(const joy::JoyConstPtr& msg)
+{
+  boost::mutex::scoped_lock(cntl_mutex_);
+  // joy_in=*msg;
+  roomba_500driver_meiji::RoombaCtrl ctrl;
 
-	if(msg->buttons[8]){	//for red
-	//if(msg->buttons[11]){
-		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::POWER;
-		cout<<"POWER OFF"<<endl;
-	}
-	/*	
+  ctrl.mode = roomba_500driver_meiji::RoombaCtrl::DRIVE;
+
+  if (msg->buttons[0])
+  {
+    ctrl.mode = roomba_500driver_meiji::RoombaCtrl::SAFE;
+    std::cout << "SAFE mode" << std::endl;
+  }
+
+  if (msg->buttons[1])
+  {
+    ctrl.mode = roomba_500driver_meiji::RoombaCtrl::SPOT;
+    std::cout << "SPOT mode" << std::endl;
+  }
+
+  if (msg->buttons[2])
+  {
+    ctrl.mode = roomba_500driver_meiji::RoombaCtrl::CLEAN;
+    std::cout << "CLEAN mode" << std::endl;
+  }
+
+  if (msg->buttons[3])
+  {
+    // ctrl.mode=roomba_500driver_meiji::RoombaCtrl::MAX;
+    // std::cout<<"MAX mode"<<std::endl;
+    ctrl.mode = roomba_500driver_meiji::RoombaCtrl::DOCK;
+    std::cout << "DOCK mode" << std::endl;
+  }
+
+  if (msg->buttons[4])
+  {
+    ctrl.mode = roomba_500driver_meiji::RoombaCtrl::MOTORS;
+    std::cout << "MOTORS mode" << std::endl;
+  }
+  if (msg->buttons[6])
+  {
+    ctrl.mode = roomba_500driver_meiji::RoombaCtrl::MOTORS_OFF;
+    std::cout << "MOTORS OFF mode" << std::endl;
+  }
+  if (msg->buttons[5])
+  {
+    ctrl.mode = roomba_500driver_meiji::RoombaCtrl::FORCE_SEEK_DOCK;
+    std::cout << "FORCE_SEEK_DOCK mode" << std::endl;
+  }
+  if (msg->buttons[7])
+  {
+    ctrl.mode = roomba_500driver_meiji::RoombaCtrl::FULL;
+    std::cout << "FULL mode" << std::endl;
+  }
+
+  if (msg->buttons[9])
+  {  // for red controller
+     // if(msg->buttons[10]){
+    ctrl.mode = roomba_500driver_meiji::RoombaCtrl::WAKEUP;
+    std::cout << "WAKEUP" << std::endl;
+  }
+
+  if (msg->buttons[8])
+  {  // for red
+     // if(msg->buttons[11]){
+    ctrl.mode = roomba_500driver_meiji::RoombaCtrl::POWER;
+    std::cout << "POWER OFF" << std::endl;
+  }
+  /*
 	ctrl.velocity=1.0*roomba_msgs::RoombaCtrl::DEFAULT_VELOCITY*msg->axes[1]+
 					roomba_msgs::RoombaCtrl::DEFAULT_VELOCITY*msg->axes[5];
-					
+
 	ctrl.radius=((float)ctrl.velocity/msg->axes[2])+0*roomba_msgs::RoombaCtrl::STRAIGHT_RADIUS;
-	
+
 	if(fabs(ctrl.radius)>roomba_msgs::RoombaCtrl::STRAIGHT_RADIUS){
 		ctrl.radius=roomba_msgs::RoombaCtrl::STRAIGHT_RADIUS;
 	}
-	
-	if(msg->axes[4]){ // 
+
+	if(msg->axes[4]){ //
 		ctrl.velocity=roomba_msgs::RoombaCtrl::DEFAULT_VELOCITY;
 		ctrl.radius=msg->axes[4];
 	}
 	*/
-	if(msg->axes[1] || msg->axes[2]){ // 
-		ctrl.cntl.linear.x=0.002*roomba_500driver_meiji::RoombaCtrl::DEFAULT_VELOCITY*msg->axes[1];
-		ctrl.cntl.angular.z=(float)msg->axes[2];
-	
-		ctrl.mode=roomba_500driver_meiji::RoombaCtrl::DRIVE_DIRECT;
-		//cout<<ctrl.velocity<<"   "<<ctrl.radius<<endl;
-		cout<<ctrl.cntl.linear.x<<"   "<<ctrl.cntl.angular.z<<endl;
-	}	
-	
-	
-	
-	pub_state.publish(ctrl);
+  if (msg->axes[1] || msg->axes[2])
+  {  //
+    ctrl.cntl.linear.x = 0.002 * roomba_500driver_meiji::RoombaCtrl::DEFAULT_VELOCITY * msg->axes[1];
+    ctrl.cntl.angular.z = static_cast<float>(msg->axes[2]);
+
+    ctrl.mode = roomba_500driver_meiji::RoombaCtrl::DRIVE_DIRECT;
+    // std::cout<<ctrl.velocity<<"   "<<ctrl.radius<<std::endl;
+    std::cout << ctrl.cntl.linear.x << "   " << ctrl.cntl.angular.z << std::endl;
+  }
+
+  pub_state.publish(ctrl);
 }
 
-int main(int argc, char** argv) {
-	
-    ros::init(argc, argv, "joystick_drive");
-    ros::NodeHandle n;
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "joystick_drive");
+  ros::NodeHandle n;
 
-	pub_state=n.advertise<roomba_500driver_meiji::RoombaCtrl>("/roomba/control", 100);
-	//ros::Publisher pub_odom= n.advertise<geometry_msgs::TransformStamped >("/global_frame", 100);
-	ros::Subscriber cntl_sub = n.subscribe("/joy", 100, joy_callback);
-	/*
+  pub_state = n.advertise<roomba_500driver_meiji::RoombaCtrl>("/roomba/control", 100);
+  // ros::Publisher pub_odom= n.advertise<geometry_msgs::TransformStamped >("/global_frame", 100);
+  ros::Subscriber cntl_sub = n.subscribe("/joy", 100, joy_callback);
+  /*
 	ros::Rate loop_rate(20);
 	while (ros::ok()) {
-		
+
 		ros::spinOnce();
         loop_rate.sleep();
     }
 	*/
-	
-	ros::spin();
-    return 0;
-}
 
+  ros::spin();
+  return 0;
+}