crashing bugs fixed

src/safe_cmd_alg_node.cpp

@@ -73,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.y  += (msg->linear.y  - last_twist_.linear.y ); 
+      Twist_msg_.linear.x  += (msg->linear.x  - last_twist_.linear.x ); 
+      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;
   }