merge spark-04 to master

junjian-zhang · Aug 23, 2017 · 6b5fc7f · 6b5fc7f
2 parents bc0f89f + 0d92948
commit 6b5fc7f
Show file tree

Hide file tree

Showing 20 changed files with 84 additions and 104 deletions.
diff --git a/spark_v04/spark/spark_bringup/launch/driver_bringup.launch b/spark_v04/spark/spark_bringup/launch/driver_bringup.launch
@@ -1,7 +1,7 @@
 <launch>
   <include file="$(find spark_description)/launch/spark_description.launch"/>
   <include file="$(find spark_base)/launch/spark_base.launch">
-	<arg name="serialport"                  value="/dev/ttyUSB0"/>
+	<arg name="serialport"                  value="/dev/sparkBase"/>
   </include>
   <include file="$(find astra_launch)/launch/astra.launch">
     <arg name="rgb_processing"                  value="true"/>

diff --git a/spark_v04/spark/spark_test/launch/spark_driver.launch b/spark_v04/spark/spark_test/launch/spark_driver.launch
@@ -1,7 +1,7 @@
 <launch>
     <include file="$(find spark_description)/launch/spark_description.launch"/>
 	<include file="$(find spark_base)/launch/spark_base.launch">
-		<arg name="serialport"                  value="/dev/ttyUSB0"/> 
+		<arg name="serialport"                  value="/dev/sparkBase"/> 
 	</include>
 
 	<include file="$(find astra_launch)/launch/astra.launch">

diff --git a/spark_v04/spark_app/spark_follower/launch/nxfollower_bringup.launch b/spark_v04/spark_app/spark_follower/launch/nxfollower_bringup.launch
@@ -3,6 +3,7 @@
  -->
 
 <launch>
-	<include file="$(find spark_follower)/launch/include/velocity_smoother.launch.xml"/>	
+	<include file="$(find spark_follower)/launch/include/velocity_smoother.launch.xml"/>
+
 	<node pkg="spark_follower" type="nxfollower" name="nxfollower"  output="screen"/>
 </launch>
diff --git a/spark_v04/spark_app/spark_follower/launch/nxfollower_bringup_nodelet.launch b/spark_v04/spark_app/spark_follower/launch/nxfollower_bringup_nodelet.launch
@@ -3,7 +3,8 @@
  -->
 
 <launch>	
-	<node pkg="nodelet" type="nodelet" name="spark_base_nodelet_manager" args="manager"/>
 	<include file="$(find spark_follower)/launch/include/velocity_smoother.launch.xml"/>
-  	<node pkg="nodelet" type="nodelet" name="nxfollower_nodelet" args="load nxfollower/NxFollowerNodelet spark_base_nodelet_manager" output="screen"/>
+
+	<node pkg="nodelet" type="nodelet" name="spark_base_nodelet_manager" args="manager"/>
+	<node pkg="nodelet" type="nodelet" name="nxfollower_nodelet" args="load nxfollower/NxFollowerNodelet spark_base_nodelet_manager" output="screen"/>
 </launch>
diff --git a/spark_v04/spark_app/spark_follower/package.xml b/spark_v04/spark_app/spark_follower/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>spark_follower</name>
-  <version>0.0.0</version>
+  <version>0.4.0</version>
   <description>The nxrobo's follower package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
@@ -15,30 +15,6 @@
   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
   <license>TODO</license>
 
-
-  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/mypackage1</url> -->
-
-
-  <!-- Author tags are optional, mutiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintianers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="[email protected]">Jane Doe</author> -->
-
-
-  <!-- The *_depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use run_depend for packages you need at runtime: -->
-  <!--   <run_depend>message_runtime</run_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>rospy</build_depend>
@@ -49,10 +25,9 @@
   <run_depend>rospy</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>nodelet</run_depend>
-  <!-- run_depend>message_runtime</run_depend-->
-  <!-- The export tag contains other, unspecified, tags -->
+
   <export>
     <!-- Other tools can request additional information be placed here -->
-	<nodelet plugin="${prefix}/plugins/nxfollower_nodelet.xml" />
+	  <nodelet plugin="${prefix}/plugins/nxfollower_nodelet.xml" />
   </export>
 </package>
diff --git a/spark_v04/spark_app/spark_follower/param/default/smoother.yaml b/spark_v04/spark_app/spark_follower/param/default/smoother.yaml
@@ -10,7 +10,7 @@ accel_lim_v: 0.3 # maximum is actually 2.0, but we push it down to be smooth
 accel_lim_w: 1
 
 # Optional parameters
-frequency: 20.0
+frequency: 30.0
 decel_factor: 1
 
 # Robot velocity feedback type:

diff --git a/spark_v04/spark_app/spark_follower/param/default/smoother_0.2base.yaml b/spark_v04/spark_app/spark_follower/param/default/smoother_0.2base.yaml
diff --git a/spark_v04/spark_app/spark_follower/src/nxfollower.hpp b/spark_v04/spark_app/spark_follower/src/nxfollower.hpp
@@ -1,6 +1,5 @@
 /*
- * Copyright (c) 2013, Willow Garage, Inc.
- * Copyright (c) 2016, SHENZHEN NXROBO Co.,LTD.
+  * Copyright (c) 2016, SHENZHEN NXROBO Co.,LTD.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -72,9 +71,6 @@ class NxFollowerNode
   double y_thre;
 
 public:
-  /*
-   * 	constructor
-   */
   NxFollowerNode(ros::NodeHandle nh, ros::NodeHandle pnh)
     : min_y_(0.1), max_y_(0.5), min_x_(-0.2), max_x_(0.2), max_z_(0.8), goal_z_(0.6), z_scale_(1.0), x_scale_(5.0)
   {
@@ -108,17 +104,10 @@ class NxFollowerNode
     cloud_sub = nhandle.subscribe<PointCloud>("/camera/depth/points", 1, &NxFollowerNode::pointCloudCb, this);
   }
 
-  /*
-   * 	destructor
-   */
   virtual ~NxFollowerNode()
   {
   }
 
-  /*
-   * callback function of point cound
-   *  @param [in] cloud
-   */
   void pointCloudCb(const PointCloud::ConstPtr &cloud)
   {
     // X,Y,Z of the centroid
@@ -162,12 +151,16 @@ class NxFollowerNode
     }
   }
 
+<<<<<<< HEAD
   /*
    *  @brief 发布底盘的速度，包括线速度和角速度
    *  @param [in] y 相对于机器人底盘base_footprint的目标点的y坐标
    *  @param [in] depth 相对于机器人底盘base_footprint的目标点的x坐标
    */
   void pubCmd(float y, float depth)
+=======
+  void pubCmd(const float &y, const float &depth)
+>>>>>>> spark-04
   {
     double curr_dist = sqrt(y * y + depth * depth);
     if (curr_dist == 0)
@@ -179,7 +172,11 @@ class NxFollowerNode
     float x_linear = 0;
     float z_angular = 0;
     float z_scale = 1.2;
+<<<<<<< HEAD
     float x_scale = 6.0;
+=======
+    float x_scale = 2.0;
+>>>>>>> spark-04
     x_linear = (depth - goal_depth_) * z_scale;
     z_angular = asin(y / curr_dist) * x_scale;
 
@@ -194,9 +191,13 @@ class NxFollowerNode
 
     cmdvel_pub.publish(cmd);
   }
+<<<<<<< HEAD
   /*
    * waiting for the end of the program
    */
+=======
+
+>>>>>>> spark-04
   void spin()
   {
     ros::spin();

diff --git a/spark_v04/spark_app/spark_follower/src/nxfollower_nodelet.cpp b/spark_v04/spark_app/spark_follower/src/nxfollower_nodelet.cpp
@@ -5,7 +5,6 @@
 #include <nodelet/nodelet.h>
 
 #include "nxfollower.hpp"
-// this should really be in the implementation (.cpp file)
 
 namespace nxfollower
 {
@@ -37,6 +36,7 @@ class NxFollowerNodelet : public nodelet::Nodelet
 // watch the capitalization carefully
 #include <pluginlib/class_list_macros.h>
 PLUGINLIB_EXPORT_CLASS(nxfollower::NxFollowerNodelet, nodelet::Nodelet)
+<<<<<<< HEAD
 // PLUGINLIB_DECLARE_CLASS(usb_cam, usb_cam_nodelet, usb_cam::usb_cam_nodelet, nodelet::Nodelet);
 
 #endif
@@ -53,3 +53,7 @@ PLUGINLIB_EXPORT_CLASS(nxfollower::NxFollowerNodelet, nodelet::Nodelet)
  return 0;
  }
  */
+=======
+
+#endif
+>>>>>>> spark-04
diff --git a/spark_v04/spark_app/spark_navigation/launch/amcl_demo.launch b/spark_v04/spark_app/spark_navigation/launch/amcl_demo.launch
@@ -5,7 +5,7 @@
 
 <!-- base -->
   <include file="$(find spark_base)/launch/spark_base.launch">
-	<arg name="serialport"                  value="/dev/ttyUSB0"/>
+	<arg name="serialport"                  value="/dev/sparkBase"/>
   </include>
 
 

diff --git a/spark_v04/spark_app/spark_navigation/launch/amcl_demo_lidar.launch b/spark_v04/spark_app/spark_navigation/launch/amcl_demo_lidar.launch
@@ -5,7 +5,7 @@
 
 <!-- base -->
   <include file="$(find spark_base)/launch/spark_base.launch">
-	<arg name="serialport"                  value="/dev/ttyUSB0"/>
+	<arg name="serialport"                  value="/dev/sparkBase"/>
   </include>
 
 

diff --git a/spark_v04/spark_app/spark_navigation/launch/gmapping_demo.launch b/spark_v04/spark_app/spark_navigation/launch/gmapping_demo.launch
@@ -6,7 +6,7 @@
 
   <!-- base -->
   <include file="$(find spark_base)/launch/spark_base.launch">
-	<arg name="serialport"                  value="/dev/ttyUSB0"/>
+	<arg name="serialport"                  value="/dev/sparkBase"/>
   </include>
 
 

diff --git a/spark_v04/spark_app/spark_navigation/launch/gmapping_demo_lidar.launch b/spark_v04/spark_app/spark_navigation/launch/gmapping_demo_lidar.launch
@@ -6,13 +6,13 @@
 
   <!-- base -->
  <include file="$(find spark_base)/launch/spark_base.launch">
-	<arg name="serialport"                  value="/dev/ttyUSB0"/>
+	<arg name="serialport"                  value="/dev/sparkBase"/>
  </include>
 
   <!-- 3D sensor -->
   <arg name="3d_sensor" default="3i_lidar"/>  
   <include file="$(find b_3ilidar)/launch/bringup.launch">
-  <!--<arg name="lidar_USB"                  value="/dev/ttyUSB0"/>-->
+  <!--<arg name="lidar_USB"                  value="/dev/sparkBase"/>-->
   </include>
 
 

diff --git a/spark_v04/spark_driver/b_3ilidar/include/b_3ilidar/rslidar_protocol.h b/spark_v04/spark_driver/b_3ilidar/include/b_3ilidar/rslidar_protocol.h
@@ -23,10 +23,17 @@
 //-----------------------------------------
 #define COMM_HEAD_FLAGE 0xAA  // Frame header
 
+<<<<<<< HEAD
 #define COMM_FRAME_TYPE_ATTR 0x20      // attribute frame
 #define COMM_FRAME_TYPE_RSP_ATTR 0x21  // attribute frame
 
 #define COMM_FRAME_TYPE_CMD 0x40      // command farme
+=======
+#define COMM_FRAME_TYPE_ATTR 0x20  // attribute frame
+#define COMM_FRAME_TYPE_RSP_ATTR 0x21  // attribute frame
+
+#define COMM_FRAME_TYPE_CMD 0x40  // command farme
+>>>>>>> spark-04
 #define COMM_FRAME_TYPE_RSP_CMD 0x41  // command farme
 
 #define COMM_FRAME_TYPE_MESSAGE 0x61  // message frame

diff --git a/spark_v04/spark_driver/b_3ilidar/launch/bringup.launch b/spark_v04/spark_driver/b_3ilidar/launch/bringup.launch
@@ -6,7 +6,7 @@
 		   <param name = "lidar_USB" value = "/dev/ttyUSB1"/>
 		</node>
 
-		 <!--  <param name = "lidar_USB" value = "/dev/ttyUSB0"/>-->
+		 <!--  <param name = "lidar_USB" value = "/dev/sparkBase"/>-->
 
 		<!--node pkg="tf" type="static_transform_publisher" name="static_transform_publisher"
 			args="0.0 0.0 0.0 0.0 0.0 0.0 1.0 laser_frame base_footprint 1000" output="screen"/-->

diff --git a/spark_v04/spark_driver/b_3ilidar/src/main.cpp b/spark_v04/spark_driver/b_3ilidar/src/main.cpp
@@ -1,3 +1,40 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2017, NXROBO Ltd.
+ *  Xiankai Chen <[email protected]>
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
 #include <ros/ros.h>
 #include <sensor_msgs/LaserScan.h>
 #include <boost/thread.hpp>
@@ -46,7 +83,11 @@ class Threeiladar
   Threeiladar(ros::NodeHandle in_nh)
   {
     nh = in_nh;
+<<<<<<< HEAD
     opt_com_path = "/dev/ttyUSB0";
+=======
+    opt_com_path = "/dev/sparkBase";
+>>>>>>> spark-04
     if (!nh.getParam("/b_3ilidar_node/lidar_USB", opt_com_path))
       ROS_ERROR("No lidar_USB param found, the serial port will be set as %s", opt_com_path.c_str());
     opt_com_baudrate = 115200;

diff --git a/spark_v04/spark_driver/base/spark_base/include/spark_base/mylock.hpp b/spark_v04/spark_driver/base/spark_base/include/spark_base/mylock.hpp
@@ -1,7 +1,6 @@
 /*
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2011, Willow Garage, Inc.
  *  Copyright (c) 2016, NXROBO Ltd.
  *  Xiankai Chen <[email protected]>
  *
@@ -67,33 +66,21 @@ class ILock
 class CMutex
 {
 public:
-  /**
-   * Constructor
-   */
   CMutex()
   {
     pthread_mutex_init(&m_mutex, NULL);
   }
 
-  /**
-   * Destructor
-   */
   ~CMutex()
   {
     pthread_mutex_destroy(&m_mutex);
   }
 
-  /**
-   * Lock
-   */
   void Lock() const
   {
     pthread_mutex_lock(&m_mutex);
   }
 
-  /**
-   *Unlock
-   */
   void Unlock() const
   {
     pthread_mutex_unlock(&m_mutex);

diff --git a/spark_v04/spark_driver/base/spark_base/include/spark_base/spark_base_interface.h b/spark_v04/spark_driver/base/spark_base/include/spark_base/spark_base_interface.h
@@ -1,7 +1,6 @@
 /*
  * Software License Agreement (BSD License)
  *
- *  Copyright (c) 2011, Willow Garage, Inc.
  *  Copyright (c) 2016, NXROBO Ltd.
  *  Litian Zhuang <[email protected]>
  *