[ros-users] Obstacles Detection with laser data

Bhaskara Marthi bhaskara at willowgarage.com
Thu Jan 20 16:17:46 UTC 2011


If you're computing the robot position using an integrator, it's not going
to be in the map frame unless the robot starts off at the origin in the map
frame, and even then it would be subject to odometric drift.  The fact that
you're able to transform the point cloud into the map frame means you have a
localization algorithm running, so you can just use tf to get the base pose
in the map frame directly.
- Bhaskara

2011/1/20 Nicolás Alvarez Picco <nicolasapicco at hotmail.com>

>  Sorry you are right!!
> I have done that, the problem is that the detection is not quite robust. I
> am adding the code that I am using, just for u to have an idea.
> I am doing all this when I receive a msgs of laser data from gazebo, this
> is a callback, is it too much code for a callback??
> In the function safety_manager->is_state_safe I am just calculating the
> distance between the new robot s state given by a runge kutta integrator and
> each of the point of the laser, previously transformed to the global frame("/map"). Has someone done this in a more efficient way??
>
> Thanks !!!
>
> Nicolas
>
> void    Kernel::Callback(const sensor_msgs::LaserScan::ConstPtr&
> cloudlaser){
>
>
>     try{
>     tlistener.waitForTransform("/map", cloudlaser->header.frame_id,
>                               cloudlaser->header.stamp,
> ros::Duration(1.0));
>
>    tlistener.lookupTransform("/map", cloudlaser->header.frame_id,
>                               cloudlaser->header.stamp, lasertransform);
>
>
>       pcl::transformAsMatrix(lasertransform, mat);
>
>   }
>     catch (tf::TransformException ex){
>       ROS_ERROR("%s",ex.what());
>   }
>
> projector_.transformLaserScanToPointCloud(cloudlaser->header.frame_id,*cloudlaser,
> cloud,tlistener);
>       sensor_msgs::convertPointCloudToPointCloud2(cloud, cloud2);
>
>       pcl::transformPointCloud(mat, cloud2, pc_toPcl);
>
>      pcl::fromROSMsg(pc_toPcl, laser_fchecker);
>      double dist_states=0,best_avg=0;
>
>        if (start_callback) {
>             pose2d_t robot_pose = ros_link->get_robot_pose();
>             start_state.pose = robot_pose;
>             start_state.vel.front = start_state.vel.ang = 0.0;
>             actual_state=start_state;
>             current_act.acc.front = 0;
>             current_act.acc.ang   = 0;
>             start_callback=false;
>
>    }
>
> if ((current_act.acc.front==0) && ( current_act.acc.ang==0)){
>     pose2d_t robot_pose = ros_link->get_robot_pose();
>             start_state.pose = robot_pose;
>             start_state.vel.front = start_state.vel.ang = 0.0;
>             actual_state=start_state;
>             current_act.acc.front = 0;
>             current_act.acc.ang   = 0;
> }
>
>    Action control, best_control;
>  bool aux=false;
>    metric_t  best_result = 0;
>
>
>       for(double fv = -0.8; fv <= 0.8;    fv += 0.2){
>           for(double av =-0.75; av <= 0.75; av += 0.25){
>             control.acc.front = fv;
>             control.acc.ang   = av;
>
>                 if ((control.acc.front == current_act.acc.front) &&
> (control.acc.ang == current_act.acc.ang))
>                     continue;
>             new_state = integrator->integrate(actual_state, control);
>
>             if (!safety_manager->is_state_safe(actual_state, new_state,
> &laser_fchecker)){  // PICS checker and Mapcheker
>                 ROS_INFO("State_No_Safe:\n");
>                 aux=false;
>                 continue;
>             }
>             else
>             aux=true;
>                  best_state = new_state;
>                 best_control.acc.front =control.acc.front ;
>                 best_control.acc.ang=control.acc.ang;
>                 best_control.end_pose=new_state.pose;
>
>   }
>
>     }
>         if (!aux){
>                             best_control.acc.front=
> best_control.acc.ang=0;
>                             best_control.end_pose=actual_state.pose;
>                             best_state=actual_state;
>                        }
>
>
>    vel2d_t cmd_vel;
>    pose2d_t robot_pose;
>    ros::Rate loop_rate(250);
>
>     current_act= best_control;
>
>          robot_pose = ros_link->get_robot_pose();
>         cmd_vel = calculate_control(current_act, robot_pose);
>          ros_link->send_cmd_vel(cmd_vel);
>     actual_state=best_state;
>       }
>
>
>
> where :
>
>
>
> bool Safety_Manager::is_state_safe (Phase_State state1, Phase_State state2,
> pcl::PointCloud<pcl::PointXYZ>* pts_laser){
>
>   robot_r = 1;
>   double dist_obstacles, dist_states,dist_obstacles1;
>
>   dist_states = dist2d(state1.pose, state2.pose);
>   int size_cloud=pts_laser->points.size();
>
>
>   for(int it=0; it<size_cloud; it++){
>
> dist_obstacles=sqrt(pow(pts_laser->points[it].x-state2.pose.x,2)+pow(pts_laser->points[it].y-state2.pose.y,2));
> //distancia entre 2 muestras consecutivas
>              if(dist_obstacles <= 1)  {// quiero decir si el centro de mi
> robot esta a mas de la distancia del r significa q esta en un estado a
> seguro!!! y 4 metros es el mx alcance de mi laser
>                  return false;
>         }
>   }
>    return true;
> }
>
> ------------------------------
> Date: Tue, 18 Jan 2011 09:33:13 -0800
> From: bhaskara at willowgarage.com
> To: ros-users at code.ros.org
> Subject: Re: [ros-users] Obstacles Detection with laser data
>
>
> Hi Nicolas, it's hard to diagnose this without more surrounding context and
> an explanation of what exactly 'not working well' means.  But one
> possibility might be that pts_laser and state2.pose are not in the same
> coordinate frame.  Have you used tf to transform the point cloud into the
> global frame?
> - Bhaskara
>
> 2011/1/18 Nicolás Alvarez Picco <nicolasapicco at hotmail.com>
>
>  Hi Everybody!!
>
> I want to detect objects, but just their position to avoid them with the
> laser data. So I am using these lines:
>
>    for(int it=0; it<size_cloud; it++){
>
> dist_obstacles=sqrt(pow(pts_laser->points[it].x-state2.pose.x,2)+pow(pts_laser->points[it].y-state2.pose.y,2));
> //distancia entre 2 muestras consecutivas
>
>         if(dist_obstacles <= robot_r)  {// quiero decir si el centro de mi
> robot esta a mas de la distancia del r significa q esta en un estado a
> seguro!!! y 4 metros es el mx alcance de mi laser
>
>             return false;
>         }
>   }
>
> where pts_laser is a pcl_pointcloud and the state2 give me the future
> position of the robot.
> So it is a checker, if there is a point which is closer to the robot than
> the radius of the robot, so that state is unsafe.
> This thing is not working well. So do you have a better solution or an
> idea??
>
> Thanks
>
> Nicolas
>
> _______________________________________________
> ros-users mailing list
> ros-users at code.ros.org
> https://code.ros.org/mailman/listinfo/ros-users
>
>
>
>
> --
> Bhaskara Marthi
> Research Scientist
> Willow Garage Inc.
> 650-475-2856
>
> _______________________________________________ ros-users mailing list
> ros-users at code.ros.org https://code.ros.org/mailman/listinfo/ros-users
>
> _______________________________________________
> ros-users mailing list
> ros-users at code.ros.org
> https://code.ros.org/mailman/listinfo/ros-users
>
>


-- 
Bhaskara Marthi
Research Scientist
Willow Garage Inc.
650-475-2856
-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://lists.ros.org/pipermail/ros-users/attachments/20110120/e8e6bd69/attachment-0003.html>


More information about the ros-users mailing list