Merge branch 'nav-10'

controller/src/TODO

@@ -1,4 +1,10 @@
 TODO
 
-> @tttor: do not compile unused plugins in mavros
-> @tttor: move rbmt_joy package to rbmt_teleop package
+@tttor:
+> make compact costumized msg types in mavlink, e.g. wheel_speed, trajectory_metadata, see http://qgroundcontrol.org/mavlink/create_new_mavlink_message
+> make costumized mavros plugins, see http://wiki.ros.org/mavros
+> implement AMCL (adaptive Monte Carlo Localization), see http://wiki.ros.org/amcl?distro=indigo
+> constraint the workspace, complete the configuration of the cost map
+> do not compile unused plugins in mavros
+> standardize source code file naming on packages
+> refine .launch, follows http://wiki.ros.org/roslaunch/Tutorials/Roslaunch%20tips%20for%20larger%20projects

controller/src/rbmt_launcher/launch/demo_m1.launch

@@ -9,7 +9,8 @@
   <!-- Set up parameter server -->
   <param name="real_rbmt" value="$(arg real_rbmt)"/>
   <param name="dt_realization" value="0.1" type="double"/> <!-- one discrete time unit dt is realized as <value> seconds -->
-  <param name='max_linear_vel' value="1.0" type="double"/> <!-- vel is w.r.t. /map and in ((meter) per (one discrete time unit dt)) -->
+  <param name='max_linear_vel' value="1.0" type="double"/> <!-- the linear vel is w.r.t. /map and in ((meter) per (one discrete time unit dt)) -->
+  <param name='max_angular_vel' value="0.785" type="double"/> <!-- the angular vel is w.r.t. x+ axis of /map and in ((radian) per (one discrete time unit dt)) -->
   <rosparam param="source_list">['act_joint_state']</rosparam>
 
   <!-- Simulator start-up -->

controller/src/rbmt_move_base/include/rbmt_move_base/global_planner.h

@@ -4,6 +4,7 @@
 #include <ros/ros.h>
 #include <geometry_msgs/Twist.h>
 #include <geometry_msgs/PoseStamped.h>
+#include <tf/transform_datatypes.h>
 #include <Eigen/Dense>
 
 #include <vector>
@@ -24,6 +25,7 @@ class GlobalPlanner {
 
  private:
   double max_linear_vel_;
+  double max_angular_vel_;
 
   template<typename T>
   double get_cartesian_dist(const T& p1, const T& p2) {
@@ -37,6 +39,9 @@ class GlobalPlanner {
 
     return sqrt(total);
   }
+
+  //! return the smallest distance, either CCW or CW
+  double get_yaw_dist(const double& y2, const double& y1);
 };
 
 }// namespace rbmt_move_base

controller/src/rbmt_move_base/src/global_planner.cpp

@@ -8,56 +8,75 @@ GlobalPlanner::GlobalPlanner() {
   const double max_linear_vel_default_val = 0.1;
   nh.param<double> ("max_linear_vel", max_linear_vel_, max_linear_vel_default_val);
 
+  const double max_angular_vel_default_val = 0.25*M_PI;  
+  nh.param<double> ("max_angular_vel", max_angular_vel_, max_angular_vel_default_val);
+
 };
 
 GlobalPlanner::~GlobalPlanner() {};
 
 bool GlobalPlanner::plan(const geometry_msgs::PoseStamped& start_pose, const geometry_msgs::PoseStamped& goal_pose, std::vector<geometry_msgs::PoseStamped>* global_plan) {
   using namespace std;
 
+  //
   Eigen::Vector2d s(start_pose.pose.position.x, start_pose.pose.position.y);
   Eigen::Vector2d g(goal_pose.pose.position.x, goal_pose.pose.position.y);
+  double s_yaw = tf::getYaw(start_pose.pose.orientation);
+  double g_yaw = tf::getYaw(goal_pose.pose.orientation);
+
+  // 
+  const double linear_dist = get_cartesian_dist<Eigen::Vector2d>(s, g);
+  const double angular_dist = get_yaw_dist(g_yaw, s_yaw);
 
-  // For now, consider only 2D pose
-  const double dist = get_cartesian_dist<Eigen::Vector2d>(s, g);
+  size_t n_segment = ((size_t) linear_dist / max_linear_vel_);
+  if (remainder(linear_dist,max_linear_vel_) > 0.0) {
+    ++n_segment;// since we desire each segment can be done in, at most, max_linear_vel_
+  }
 
-  size_t n_segment = ((size_t) dist / max_linear_vel_);
-  if (remainder(dist,max_linear_vel_) > 0.0) {
-    ++n_segment;// since we desire each segment can be done in,at most, max_linear_vel_
+  if ((std::abs(angular_dist)/n_segment) > max_angular_vel_) {
+    n_segment = ((size_t) angular_dist / max_angular_vel_);
+    if (remainder(angular_dist,max_angular_vel_) > 0.0) {
+      ++n_segment;// since we desire each segment can be done in, at most, max_angular_vel_
+    }    
   }
 
+  //
   Eigen::Vector2d segment;
   segment = (g - s) / n_segment;
 
+  double angular_segment;
+  angular_segment = angular_dist / n_segment;
+
+  //
   global_plan->clear();
   global_plan->push_back(start_pose);
   for (size_t i=0; i<n_segment; ++i) {
+    size_t mul = i+1;// plus one as w is on the tip of i-th segment
+
     Eigen::Vector2d w;
-    w = s + ((i+1)*segment);// plus one as w is on the tip of i-th segment
+    w = s + (mul*segment);
+
+    double angular_w;
+    angular_w = s_yaw + (mul*angular_segment);
 
     geometry_msgs::PoseStamped spose;
     spose.header = start_pose.header;// TODO fix me
     spose.pose.position.x = w(0);
     spose.pose.position.y = w(1);
     spose.pose.position.z = 0.0;
-    spose.pose.orientation.x = 0.0;
-    spose.pose.orientation.y = 0.0;
-    spose.pose.orientation.z = 0.0;
-    spose.pose.orientation.w = 1.0;
+    spose.pose.orientation = tf::createQuaternionMsgFromYaw(angular_w);
 
     global_plan->push_back(spose);
   }
 
-  // for (size_t i=0; i<global_plan->size(); ++i) {
-  //   geometry_msgs::PoseStamped spose;
-  //   spose = global_plan->at(i);
-
-  //   cout << "spose.pose.position.x= " << spose.pose.position.x << endl;
-  // }
-
   return true;
 }
 
+double GlobalPlanner::get_yaw_dist(const double& y2, const double& y1) {
+  double dist = y2 - y1;
 
+  if (dist > M_PI) return -1.0 * (y1 + (2*M_PI-y2));// multiplied by -1.0 for CW rotation
+  else return dist;
+}
 
 }// namespace rbmt_move_base

controller/src/rbmt_tracking/src/tracker.cpp

@@ -69,7 +69,7 @@ void Tracker::run_dummy(ros::Rate rate) {
   pose.position.x = 3.0;
   pose.position.y = 3.0;
   pose.position.z = 0.0;
-  pose.orientation.x = 0.0;
+  pose.orientation.x = 0.0im;
   pose.orientation.y = 0.0;
   pose.orientation.z = 0.0;
   pose.orientation.w = 1.0;
@@ -92,19 +92,51 @@ void Tracker::run_dummy(ros::Rate rate) {
 
   size_t idx = 0;
   while (ros::ok())  {
-    geometry_msgs::PoseStamped spose;
-    spose.header = header;
-    spose.pose = poses.at(idx%poses.size());
-
-    marker.pose = spose.pose;
-
-    cock_pose_pub_.publish(spose);
-    marker_pub_.publish(marker);
-
-    ++idx;
-    ros::spinOnce();
-    rate.sleep();
-  }
+    // // Generate the pose of cock's end randomly
+    // const double x_max, x_min, y_max, y_min;
+    // x_min = 0.0;
+    // y_min = 0.0;
+    // x_max = 8.500;
+    // y_max = 16.400;
+
+    // double x, y;
+    // bool passed = false;
+    // while (!passed) {
+    //   x = random();// TODO fix me
+
+    //   if ((x<x_min) or (x>x_max)) continue;
+    //   else break;
+    // }
+    // while (!passed) {
+    //   y = random();
+
+    //   if ((y<y_min) or (y>y_max)) continue;
+    //   else break;
+    // }
+
+  //   //
+  //   geometry_msgs::Pose pose;
+  //   pose.position.x = x;
+  //   pose.position.y = y;
+  //   pose.orientation.x = 0.0;
+  //   pose.orientation.y = 0.0;
+  //   pose.orientation.z = 0.0;
+  //   pose.orientation.w = 1.0;
+
+  //   //
+  //   geometry_msgs::PoseStamped spose;
+  //   spose.header = header;
+  //   spose.pose = pose;
+
+  //   marker.pose = spose.pose;
+
+  //   cock_pose_pub_.publish(spose);
+  //   marker_pub_.publish(marker);
+
+  //   ++idx;
+  //   ros::spinOnce();
+  //   rate.sleep();
+  // }
 }
 
 void Tracker::marker_init() {

robot/TODO

@@ -0,0 +1,4 @@
+TODO
+
+@tttor:
+