No commit message

No commit message

src/safe_cmd_alg_node.cpp

@@ -2,8 +2,6 @@
 
 SafeCmdAlgNode::SafeCmdAlgNode(void) :
   algorithm_base::IriBaseAlgorithm<SafeCmdAlgorithm>(),
-  front_obstacle_(false),
-  rear_obstacle_(false),
   collision_time_(1),
   min_dist_(0.3),
   max_vel_front_(10),
@@ -75,12 +73,12 @@ void SafeCmdAlgNode::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
   {
     if(Twist_msg_.linear.x < max_vel_front_ && Twist_msg_.linear.x > max_vel_rear_ )
     {
-      Twist_msg_.linear.x  =+ msg->.linear.x  - last_twist_.linear.x ; 
-      Twist_msg_.linear.y  =+ msg->.linear.y  - last_twist_.linear.y ; 
-      Twist_msg_.linear.z  =+ msg->.linear.z  - last_twist_.linear.z ; 
-      Twist_msg_.angular.x =+ msg->.angular.x - last_twist_.angular.x; 
-      Twist_msg_.angular.y =+ msg->.angular.y - last_twist_.angular.y; 
-      Twist_msg_.angular.z =+ msg->.angular.z - last_twist_.angular.z; 
+      Twist_msg_.linear.x  =+ msg->linear.x  - last_twist_.linear.x ; 
+      Twist_msg_.linear.y  =+ msg->linear.y  - last_twist_.linear.y ; 
+      Twist_msg_.linear.z  =+ msg->linear.z  - last_twist_.linear.z ; 
+      Twist_msg_.angular.x =+ msg->angular.x - last_twist_.angular.x; 
+      Twist_msg_.angular.y =+ msg->angular.y - last_twist_.angular.y; 
+      Twist_msg_.angular.z =+ msg->angular.z - last_twist_.angular.z; 
     }
     last_twist_= *msg;
   }
@@ -154,4 +152,6 @@ float SafeCmdAlgNode::compute_max_velocity_(const sensor_msgs::LaserScan::ConstP
     if (scan->ranges[i] / collision_time_ < max_velocity)
       max_velocity = scan->ranges[i] / collision_time_;
   }
+
+  return max_velocity;
 }