diff --git a/src/safe_cmd_alg_node.cpp b/src/safe_cmd_alg_node.cpp index 6c9afa077d4c05280a1c0950343ccb508699f036..33106c092a8cc7280d962845048b96f2feeee900 100644 --- a/src/safe_cmd_alg_node.cpp +++ b/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; }