make the driver as 0.4 base and add the teleop node's launch file

spark_v03/spark/spark_teleop/CMakeLists.txt → spark_v04/spark/spark_teleop/CMakeLists.txt

@@ -51,3 +51,8 @@ include_directories(include
    FILES_MATCHING PATTERN "*.h"
    PATTERN ".svn" EXCLUDE
  )
+
+ install(DIRECTORY
+  launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

spark_v03/spark_driver/base/spark_base/CMakeLists.txt → spark_v04/spark_driver/base/spark_base/CMakeLists.txt

@@ -13,7 +13,8 @@ add_message_files(
   FILES
   GyroMessage.msg
   SparkBaseOdom.msg
-  SparkBaseIRBumperCliff.msg
+  SparkBaseDock.msg
+  SparkBaseSensor.msg
  )
 
 generate_messages(

spark_v03/spark_driver/base/spark_base/include/spark_base/spark_base_interface.h → spark_v04/spark_driver/base/spark_base/include/spark_base/spark_base_interface.h

@@ -71,19 +71,22 @@ namespace nxsparkbase
 #define VIRTUAL_WALL 162
 
 // Positions
-#define LEFT 0
-#define RIGHT 1
-#define FRONT_LEFT 2
-#define FRONT_RIGHT 3
-#define CENTER_LEFT 4
-#define CENTER_RIGHT 5
-#define FRONT 6
-#define BACK 7
-#define ENDPOS 8
-
-#define OMNI 2
-#define MAIN_BRUSH 2
-#define SIDE_BRUSH 3
+#define LEFT                		0
+#define RIGHT				1
+#define FRONT_LEFT			2
+#define FRONT_RIGHT			3
+#define CENTER_LEFT			4
+#define CENTER_RIGHT			5
+#define FRONT				6
+#define BACK                		7
+#define CENTER              		8
+#define BACK_LEFT			9
+#define BACK_RIGHT			10
+#define ENDPOS				11
+
+#define OMNI				2
+#define MAIN_BRUSH			2
+#define SIDE_BRUSH			3
 
 // Buttons
 #define BUTTON_CLOCK 7
@@ -275,6 +278,7 @@ class OpenInterface
   bool bumper_[ENDPOS];     //! Bumper sensors. Indexes: LEFT RIGHT
   bool ir_bumper_[ENDPOS];  //! IR bumper sensors. Indexes: LEFT FRONT_LEFT
   // CENTER_LEFT CENTER_RIGHT FRONT_RIGHT RIGHT
+  bool wheel_over_current_[2];				//! Wheel current over: Indexes: LEFT RIGHT
   bool wheel_drop_[2];   //! Wheel drop sensors: Indexes: LEFT RIGHT
   bool omni_[ENDPOS];    //! omni wheel sensors: Indexes: FRONT BACK
   int wall_signal_;      //! Wall signal.
@@ -308,8 +312,13 @@ class OpenInterface
   float charge_;      //! Battery charge in Ah.
   float capacity_;    //! Battery capacity in Ah
 
+  bool search_dock_;              //! search dock 
+  bool touch_charge_;             //! touch charge
+  bool plug_charge_;              //! plug charge
+  bool dock_direction_[ENDPOS];   //! dock direction 
+
   int stasis_;             //! 1 when the robot is going forward, 0 otherwise
-  unsigned int time_diff;  //! diff time.
+  unsigned int current_time;  //! diff time.
 private:
   //! Parse data
   /*!

spark_v04/spark_driver/base/spark_base/msg/SparkBaseDock.msg

@@ -0,0 +1,12 @@
+bool search_dock
+bool touch_charge
+bool plug_charge
+
+bool dock_dir_left
+bool dock_dir_right
+bool dock_dir_front
+bool dock_dir_BACK
+
+
+
+

spark_v04/spark_driver/base/spark_base/msg/SparkBaseSensor.msg

@@ -0,0 +1,20 @@
+bool ir_bumper_left
+bool ir_bumper_right
+bool ir_bumper_front_left
+bool ir_bumper_front_right
+bool ir_bumper_front
+bool ir_bumper_back_left
+bool ir_bumper_back_right
+
+
+bool cliff_left
+bool cliff_right
+bool cliff_front_left
+bool cliff_front_right
+bool cliff_back_left
+bool cliff_back_right
+
+bool wheel_drop_left
+bool wheel_drop_right
+bool wheel_over_current_left
+bool wheel_over_current_right

spark_v03/spark_driver/base/spark_base/src/spark_base.cpp → spark_v04/spark_driver/base/spark_base/src/spark_base.cpp

@@ -58,7 +58,9 @@
 #include "spark_base/kfilter.hpp"
 #include "spark_base/mylock.hpp"
 #include "spark_base/spark_base_interface.h"
-#include "spark_base/SparkBaseIRBumperCliff.h"
+#include "spark_base/SparkBaseSensor.h"
+#include "spark_base/SparkBaseDock.h"
+
 #include "spark_base/GyroMessage.h"
 #include "spark_base/SparkBaseOdom.h"
 
@@ -79,7 +81,8 @@ class ComDealDataNode
   std::string base_frame_id;
   std::string odom_frame_id;
   ros::Publisher odom_pub;
-  ros::Publisher irbumpercliff_pub;
+  ros::Publisher rb_sensor_pub;
+  ros::Publisher rb_dock_pub;
   tf::TransformBroadcaster tf_broadcaster;
   ros::Subscriber cmd_vel_sub;
   ros::Publisher gyro_pub;
@@ -227,13 +230,14 @@ void ComDealDataNode::dealMessageSwitch(unsigned char *recvbuf)
 {
   int curr_idx = (idx + COUNT_TIMES - 1) % COUNT_TIMES;
 
-  current_time = ros::Time::now();
-  fb_time[curr_idx] = current_time.toSec();
+  current_time = ros::Time::now();//ros time
+
+  fb_time[curr_idx] = sparkbase->current_time;//set spark base time which is different from ros time
   double odometry_x_ = sparkbase->odometry_x_;
   double odometry_y_ = sparkbase->odometry_y_;
   sparkbase->parseComInterfaceData(recvbuf, 0);
-  if ((sizeof(float) * 12) == (recvbuf[2] - 21 - 3))
-    pubGyroMessage(recvbuf + 21, sizeof(float) * 12);
+  if ((sizeof(float) * 12) == (recvbuf[2] - 22 - 3))
+    pubGyroMessage(recvbuf + 22, sizeof(float) * 12);
 
   sparkbase->calculateOdometry();
   if (true)
@@ -284,7 +288,8 @@ void ComDealDataNode::dealMessageSwitch(unsigned char *recvbuf)
 
   odom_x[curr_idx] = est_x;
   odom_yaw[curr_idx] = sparkbase->odometry_yaw_;
-  dt = (fb_time[curr_idx] - fb_time[idx]);
+
+  dt = (fb_time[curr_idx] - fb_time[idx])*0.0001;
   vel_x_list[curr_idx] = (odom_x[curr_idx] - odom_x[idx]) / dt;
   vel_x = 0;
   for (int i = 0; i < COUNT_TIMES; i++)
@@ -336,22 +341,43 @@ void ComDealDataNode::dealMessageSwitch(unsigned char *recvbuf)
   // publish wheel joint state
   pubWheelJointStates(vel_x, vel_yaw);
 
-  // publish irbumper
-  spark_base::SparkBaseIRBumperCliff irbumpercliff;
-  irbumpercliff.ir_bumper_left = sparkbase->ir_bumper_[LEFT];
-  irbumpercliff.ir_bumper_center_left = sparkbase->ir_bumper_[CENTER_LEFT];
-  irbumpercliff.ir_bumper_front = sparkbase->ir_bumper_[FRONT];
-  irbumpercliff.ir_bumper_center_right = sparkbase->ir_bumper_[CENTER_RIGHT];
-  irbumpercliff.ir_bumper_right = sparkbase->ir_bumper_[RIGHT];
-
-  irbumpercliff.bumper_left = sparkbase->bumper_[LEFT];
-  irbumpercliff.bumper_right = sparkbase->bumper_[RIGHT];
-
-  irbumpercliff.cliff_left = sparkbase->cliff_[LEFT];
-  irbumpercliff.cliff_center_left = sparkbase->cliff_[CENTER_LEFT];
-  irbumpercliff.cliff_center_right = sparkbase->cliff_[CENTER_RIGHT];
-  irbumpercliff.cliff_right = sparkbase->cliff_[RIGHT];
-  irbumpercliff_pub.publish(irbumpercliff);
+    //publish irbumper
+    spark_base::SparkBaseSensor sensor_msg;
+
+    sensor_msg.ir_bumper_left = sparkbase->ir_bumper_[LEFT];
+    sensor_msg.ir_bumper_front_left = sparkbase->ir_bumper_[FRONT_LEFT];
+    sensor_msg.ir_bumper_front = sparkbase->ir_bumper_[FRONT];
+    sensor_msg.ir_bumper_front_right = sparkbase->ir_bumper_[FRONT_RIGHT];
+    sensor_msg.ir_bumper_right = sparkbase->ir_bumper_[RIGHT];
+    sensor_msg.ir_bumper_back_left = sparkbase->ir_bumper_[BACK_LEFT];
+    sensor_msg.ir_bumper_back_right = sparkbase->ir_bumper_[BACK_RIGHT];
+
+
+    sensor_msg.cliff_left = sparkbase->cliff_[LEFT];
+    sensor_msg.cliff_front_left = sparkbase->cliff_[FRONT_LEFT];
+    sensor_msg.cliff_front_right = sparkbase->cliff_[FRONT_RIGHT];
+    sensor_msg.cliff_right = sparkbase->cliff_[RIGHT];
+    sensor_msg.cliff_back_left = sparkbase->cliff_[BACK_LEFT];
+    sensor_msg.cliff_back_right = sparkbase->cliff_[BACK_RIGHT];
+
+    sensor_msg.wheel_drop_left = sparkbase->wheel_drop_[LEFT];
+    sensor_msg.wheel_drop_right = sparkbase->wheel_drop_[RIGHT];
+    sensor_msg.wheel_over_current_left = sparkbase->wheel_over_current_[LEFT];
+    sensor_msg.wheel_over_current_right = sparkbase->wheel_over_current_[RIGHT];
+
+    rb_sensor_pub.publish(sensor_msg);
+
+    spark_base::SparkBaseDock dock_msg;
+    dock_msg.search_dock = sparkbase->search_dock_;
+    dock_msg.touch_charge = sparkbase->touch_charge_;
+    dock_msg.plug_charge = sparkbase->plug_charge_;
+
+    dock_msg.dock_dir_left = sparkbase->dock_direction_[LEFT];
+    dock_msg.dock_dir_right = sparkbase->dock_direction_[RIGHT];
+    dock_msg.dock_dir_front = sparkbase->dock_direction_[FRONT];
+    dock_msg.dock_dir_BACK = sparkbase->dock_direction_[BACK];
+
+    rb_dock_pub.publish(dock_msg);
 }
 
 unsigned char checkSum(unsigned char *buf)
@@ -367,9 +393,9 @@ unsigned char checkSum(unsigned char *buf)
 
 void getSparkbaseComData(char *buf, int len)
 {
-	int i,j;
-	static int count=0;
-	long long timediff;
+    int i,j;
+    static int count=0;
+    long long timediff;
     unsigned char tmpbuf[255];
     static unsigned char recvbuf[255];
     ros::Time currenttime;
@@ -382,18 +408,18 @@ void getSparkbaseComData(char *buf, int len)
     }
     currenttime = ros::Time::now();
 
-	if(count == 0)
-	{
+    if(count == 0)
+    {
         headertime = currenttime;
-	}
+    }
     timediff = (currenttime - headertime).toNSec();
 
-	if(timediff > SPARKBASETIMEOUT)
-	{
-		count = 0;
+    if(timediff > SPARKBASETIMEOUT)
+    {
+        count = 0;
         ROS_ERROR("nx-base time out-%lld\n",timediff);
         headertime = currenttime;
-	}
+    }
     if((len+count) > 255)
     {
         count = 0;
@@ -444,55 +470,62 @@ void getSparkbaseComData(char *buf, int len)
         if(count > 3)
         {
             unsigned int framelen = recvbuf[2];
-            if(count >= framelen)
+            if(recvbuf[2]<6)
+            {
+                count = 0;
+            }
+            else
             {
+                if(count >= framelen)
+                {
 
-                #if 0
-                for(j=0;j<framelen; j++)
-                    printf("%02X ",(unsigned char)recvbuf[j]);
-                printf("\n");
-                #endif
+                    #if 0
+                    for(j=0;j<framelen; j++)
+                        printf("%02X ",(unsigned char)recvbuf[j]);
+                    printf("\n");
+                    #endif
 
-                if((recvbuf[0]==0x53) && (recvbuf[1]==0x4b) &&(recvbuf[framelen-2]==0x0d) && (recvbuf[framelen-1]==0x0a))	//check the header and end
-                {
-                    if(checkSum(recvbuf) == recvbuf[framelen-3])
+                    if((recvbuf[0]==0x53) && (recvbuf[1]==0x4b) &&(recvbuf[framelen-2]==0x0d) && (recvbuf[framelen-1]==0x0a))	//check the header and end
                     {
-                        if((recvbuf[3]==0x00) && (recvbuf[4]==0x59))	//inquire
+                        if(checkSum(recvbuf) == recvbuf[framelen-3])
+                        {
+                            if((recvbuf[3]==0x00) && (recvbuf[4]==0x59))	//inquire
+                            {
+                                cddn->dealMessageSwitch(recvbuf);
+                            }
+                        }
+                        else
                         {
-                            cddn->dealMessageSwitch(recvbuf);
+                            ROS_ERROR("sparkbase-check sum error");
+                            #if 0
+                            for(j=0;j<framelen; j++)
+                                printf(L_RED "%02X " NONE ,(unsigned char)recvbuf[j]);
+                            printf("\n");
+                            #endif
                         }
+
                     }
                     else
                     {
-                        ROS_ERROR("sparkbase-check sum error");
+
+
+                        ROS_ERROR("sparkbase-header or ender error error");
                         #if 0
-                        for(j=0;j<framelen; j++)
+                        for(j=0; j<framelen; j++)
                             printf(L_RED "%02X " NONE ,(unsigned char)recvbuf[j]);
                         printf("\n");
                         #endif
                     }
-
-                }
-                else
-                {
-
-
-                    ROS_ERROR("sparkbase-header or ender error error");
-                    #if 0
-		    for(j=0; j<framelen; j++)
-                        printf(L_RED "%02X " NONE ,(unsigned char)recvbuf[j]);
-                    printf("\n");
-		    #endif
-                }
-                if(count > framelen)
-                {
-                    memcpy(tmpbuf, recvbuf+framelen, count-framelen);
-                    memcpy(recvbuf, tmpbuf, count-framelen);
-                    count = count-framelen;
-                    headertime = currenttime;
-                    goto BACKCHECK;
+                    if(count > framelen)
+                    {
+                        memcpy(tmpbuf, recvbuf+framelen, count-framelen);
+                        memcpy(recvbuf, tmpbuf, count-framelen);
+                        count = count-framelen;
+                        headertime = currenttime;
+                        goto BACKCHECK;
+                    }
+                    count = 0;
                 }
-                count = 0;
             }
         }
     }
@@ -529,7 +562,8 @@ ComDealDataNode::ComDealDataNode(ros::NodeHandle _n, const char *new_serial_port
   this->odom_reset_sub =
       this->n.subscribe<spark_base::SparkBaseOdom>("/spark_base/odom/reset", 1, &ComDealDataNode::resetOdomCb, this);
   this->gyro_pub = n.advertise<spark_base::GyroMessage>("/spark_base/gyro", 1);
-  this->irbumpercliff_pub = this->n.advertise<spark_base::SparkBaseIRBumperCliff>("/spark_base/ir_bumper_cliff", 1);
+  this->rb_sensor_pub = this->n.advertise<spark_base::SparkBaseSensor>("/spark_base/sensor", 1);        //ir_bumper_cliff
+  this->rb_dock_pub = this->n.advertise<spark_base::SparkBaseDock>("/spark_base/dock", 1);
   this->current_time = ros::Time::now();
   this->last_time = ros::Time::now();
   stimer = n.createTimer(ros::Duration(1), &ComDealDataNode::checkSerialGoon, this);