zhoujoey / stepback_recovery Goto Github PK

while(sim_dist < safty_check_dist){
      local_costmap_->getRobotPose(global_pose);
      /*
      //todo : get global pose back sim_dist of base_link
      tf::TransformListener transform_listener;
      try{
        tf::Stamped<tf::Pose> global_pose;
        tf::Stamped<tf::Pose> local_pose(tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(-1 * sim_dist, 0, 0.0)), ros::Time(0), "base_link");
        transform_listener.transformPose ("map", local_pose, global_pose);
      }catch (tf::TransformException ex){
        ROS_ERROR("%s",ex.what());
        ros::Duration(1.0).sleep();
      }
      */
      double x = global_pose.getOrigin().x()- sim_dist , y = global_pose.getOrigin().y();
      double theta = tf::getYaw(global_pose.getRotation());
      double footprint_cost = world_model_->footprintCost(x, y, theta, local_costmap_->getRobotFootprint(), 0.0, 0.0);
      if(footprint_cost < 0.0){
        geometry_msgs::Twist zero_vel;
        vel_pub.publish(zero_vel);
        r.sleep();  
        ROS_ERROR("Stepback recovery can't stepback because there is a potential collision. Cost: %.2f", footprint_cost);
        return;
      }
      sim_dist += sim_step;
    }``` 

while(sim_dist < safty_check_dist){
      local_costmap_->getRobotPose(global_pose);
      /*
      //todo : get global pose back sim_dist of base_link
      tf::TransformListener transform_listener;
      try{
        tf::Stamped<tf::Pose> global_pose;
        tf::Stamped<tf::Pose> local_pose(tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(-1 * sim_dist, 0, 0.0)), ros::Time(0), "base_link");
        transform_listener.transformPose ("map", local_pose, global_pose);
      }catch (tf::TransformException ex){
        ROS_ERROR("%s",ex.what());
        ros::Duration(1.0).sleep();
      }
      */
      double x = global_pose.getOrigin().x()- sim_dist , y = global_pose.getOrigin().y();
      double theta = tf::getYaw(global_pose.getRotation());
      double footprint_cost = world_model_->footprintCost(x, y, theta, local_costmap_->getRobotFootprint(), 0.0, 0.0);
      if(footprint_cost < 0.0){
        geometry_msgs::Twist zero_vel;
        vel_pub.publish(zero_vel);
        r.sleep();  
        ROS_ERROR("Stepback recovery can't stepback because there is a potential collision. Cost: %.2f", footprint_cost);
        return;
      }
      sim_dist += sim_step;
    }``` @zhoujoey