if fmincon did not converge the angle computations are not executed, …

…additionally if this happens for more than 3 times matching gets reinitialized

commented everything within joint_callback out
stair model size has been updated
changed dx reinitialization to only happen if the stair is acctually curved

CHANGELOG.rst

@@ -5,31 +5,43 @@ Changelog for package scalaser
 TODO
 ------------------
 * add kp to plot
+* change matlab plots to change data setXdata, setYdata
+* define beta threshold in launch file
+* add folder for figures
 
 KNOWN BUGS
 ------------------
 * Fix inconsistent performance of the angle node. It ranges from 1 Hz to 4 Hz. If a rviz is started the frequency takes a hit, however once it's closed again the node wont performe as well as before.
+* joint_states influence towards the initial guesses causes a big mess for the stair_model
 
 
 NICE TO HAVE
 ------------------
+* add radius-tracking
+* add 3D model for curved stairs
+* field of view initialization
+* add field of view in rviz
 * clean up v_r_1 and dx_1 in angle.cpp
 * add initialization for start and end of stairs to determine when to stop the stair model
 * add timestamp to published messages
 * make fov_s, fov_d dependant of the lambda position received from the MyRIO
 * make stair growth dependant of fov_s
-* define beta threshold in launch file
 * make the velocity_forward sent to the MyRio adjustable with dynamic reconfigure
 * (use IMU values to set guess for beta threshold value -> not sure if needed, since the wheelchair never comes close to a beta=~24°)
 * (change the initialization of v0 from within the angle constructor to the matching constructor since startvalues for both sides are identical)
 * (change tf static to tf2 static)
 
+0.1.00 (2015-06-12)
+-------------------
+* if fmincon did not converge the angle computations are not executed, additionally if this happens for more than 3 times matching gets reinitialized
+* commented everything within joint_callback out
+* stair model size has been updated
+* changed dx reinitialization to only happen if the stair is acctually curved 
 
 0.0.14 (2015-06-10)
 -------------------
 * added filter for scan data to be between 0.3 and 1 m
 * changed beta threshold
-* 
 
 0.0.14 (2015-06-08)
 -------------------

launch/angle.launch

@@ -2,7 +2,7 @@
 <launch>
 
   <param name="/scalaser/fov_s" type="int" value="250" />
-  <param name="/scalaser/fov_d" type="int" value="150" />
+  <param name="/scalaser/fov_d" type="int" value="180" />
   <param name="/scalaser/dzi" type="double" value=".62" />
   <param name="/scalaser/phi" type="double" value="-35" />
   <param name="/scalaser/kp" type="double" value="0.05" />
@@ -11,4 +11,6 @@
   <node pkg="scalaser" type="Angle" name="angle" output="screen" >
   </node>
 
+  <node pkg="tf" type="static_transform_publisher" name="stair_dummy" args="0 0 0 0 -.5 0 stair_middle stair_middle_upright 10" />
+
 </launch>

matlab/stairparam.m

@@ -13,7 +13,7 @@
 % [v_r, se_r] = fminsearch(handle,v0_,search_options);
 [v_r, se_r] = fmincon(handle, v0_, [], [], [], [], lb, ub, [],con_options);
 
-
+    % subplot(2,1,hs)
     % plot(xi, zi, 'x');
     % axis equal tight
     % hold on;

src/angle.cpp

@@ -1,10 +1,11 @@
 #include "angle.h"
 
 Angle::Angle(ros::NodeHandle n_):
-n(n_), a(.7), v_s(5), beta_old(0), beta_new(0), v0(5), count(0), wrong_beta_count(0),
+n(n_), a(.7), v_s(5), beta_old(0), beta_new(0), v0(5), count(0), wrong_beta_count(0), wrong_se_r_count(0)
 r_h(.1016), s(.653837), phi_f(.193897),
 kp(0), vel_fwd(0),
-v_r_1(2), v_r_2(2)
+v_r_1(2), v_r_2(2),
+alpha(0)
 {
   setParameters();
   initializePlotEngine();
@@ -86,10 +87,10 @@ bool Angle::alignWheelchair(scalevo_msgs::Starter::Request& request, scalevo_msg
     // stop main loop timer
     main_timer.stop();
 
-    ROS_INFO("Wheelchair alignment has been stopped.");
     ROS_INFO("Nr of computations: %d", count);
     ROS_INFO("Duration:           %f", ros::Time::now().toSec() - time_start);
     ROS_INFO("Average frequency:  %f Hz",count/(ros::Time::now().toSec() - time_start));
+    ROS_INFO("Wheelchair alignment has been stopped.");
 
     plotData(beta_vector);
     plot_engine.executeCommand("savefig(datestr(now))");
@@ -117,11 +118,17 @@ void Angle::timerCallback(const ros::TimerEvent& event) {
     v_r_1 = cloud_1.matchTemplate();
     v_r_2 = cloud_2.matchTemplate();
 
-    computeAngle();
-    computeStair();
+    if (cloud_1.getSe_r() < threshold && cloud_2.getSe_r() < threshold) {
+      computeAngle();
+      computeStair();
+      pubStairParameters();
+      pubEdge();
+    }
+    else {
+      ROS_WARN("Nothing published because fmincon did not converge.");
+    }
+
     computeVelocity();
-    pubStairParameters();
-    pubEdge();
 
     ROS_INFO("Callback time:        %f",event.profile.last_duration.toSec());
     count++;
@@ -133,16 +140,16 @@ void Angle::timerCallback(const ros::TimerEvent& event) {
 }
 
 void Angle::jointCallback(const sensor_msgs::JointState::ConstPtr& joint_state) {
-  phi0 = -joint_state -> position[0];
-  dzi = r_h + s*sin(-phi0 + phi_f);
+  // phi0 = -joint_state -> position[0];
+  // dzi = r_h + s*sin(-phi0 + phi_f);
 
   // to only set parameters after reinitialization comment this code and write phi0 and dzi to the parameter server instead
   // cloud_1.setParameters(phi0, dzi, fov_s, ofv_d);
   // cloud_2.setParameters(phi0, dzi, 811-fov_s-fov_d, fov_d);
   // ros::param::set("/scalaser/phi", PI/180*phi0);
   // ros::param::set("/scalaser/dzi", dzi);
 
-  ROS_INFO("Matching Parameters have been updated.");
+  // ROS_INFO("Matching Parameters have been updated.");
 }
 
 void Angle::initializeMatching() {
@@ -187,18 +194,19 @@ void Angle::computeAngle() {
   diag_1 = cloud_1.getDiag();
   diag_2 = cloud_2.getDiag();
 
-// Experimental for Curved
-  if (fabs(dx_1) > 0.8*diag_1 || fabs(dx_2) > 0.8*diag_2) {
-    v0 = cloud_1.getV_r();
-    v0(2) = 0;
-    setBoundaries();
-    dx_1 = cloud_1.getDx();
-    dx_2 = cloud_2.getDx();
-    diag_1 = cloud_1.getDiag();
-    diag_2 = cloud_2.getDiag();
-    ROS_INFO("------Edge has been changed!------");
+// Experimental for Curve
+  if (fabs(alpha) > 1) {
+    if (fabs(dx_1) > 0.8*diag_1 || fabs(dx_2) > 0.8*diag_2) {
+      v0 = cloud_1.getV_r();
+      v0(2) = 0;
+      setBoundaries();
+      dx_1 = cloud_1.getDx();
+      dx_2 = cloud_2.getDx();
+      diag_1 = cloud_1.getDiag();
+      diag_2 = cloud_2.getDiag();
+      ROS_INFO("------Edge has been changed!------");
+    }
   }
-
   computeAlpha();
   // updateFoV();
   // setFoV();
@@ -237,7 +245,7 @@ void Angle::computeBeta() {
   ROS_INFO("BETA_INTERP:   %f°", beta_new);
 
   // if (fabs(beta_old - beta_new) < 15 && fabs(beta_old) < 10) {
-  if (fabs(beta_new) < 15 && cloud_1.getSe_r() < threshold && cloud_2.getSe_r() < threshold) {
+  if (fabs(beta_new) < 15) {
 
     beta.data = beta_new;
     pub_1.publish(beta);
@@ -298,11 +306,13 @@ void Angle::computeVelocity() {
   if(cloud_1.getSe_r() < threshold && cloud_2.getSe_r() < threshold) {
     buff_2 << - beta.data * kp;
     velo.data += buff_2.str();
+    wrong_se_r_count = 0;
   }
   else {
+    wrong_se_r_count++;
     buff_2 << 0;
     velo.data += buff_2.str();
-    ROS_WARN("No velocity published since matching didn't work properly.");
+    if (wrong_se_r_count > 3) initializeMatching();
   }
   pub_s_velocity.publish(velo);
 }

src/angle.h

@@ -107,8 +107,6 @@ class Angle{
   Eigen::VectorXd v_r_1;
   Eigen::VectorXd v_r_2;
 
-
-
   // Parameters for motor controler
   double kp;
   double vel_fwd;
@@ -117,9 +115,7 @@ class Angle{
   // counter
   int count;
   int wrong_beta_count;
-
-  // Marker
-
+  int wrong_se_r_count;
 
 public:
   Angle(ros::NodeHandle n_);

src/stair_model.cpp

@@ -100,7 +100,7 @@ void StairModel::setPosition() {
   }
 
   geometry_msgs::Point p;
-  for (int i=floor(stair_bot); i < ceil(stair_top)+3; i++) {
+  for (int i=floor(stair_bot); i < ceil(stair_top) + 2; i++) {
   p.x = 0 - h*i;
   p.y = 0;
   p.z = 0 - t*i;