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.<br>
- Bhaskara<br><br><div class="gmail_quote">2011/1/20 Nicolįs Alvarez Picco <span dir="ltr"><<a href="mailto:nicolasapicco@hotmail.com">nicolasapicco@hotmail.com</a>></span><br><blockquote class="gmail_quote" style="margin: 0pt 0pt 0pt 0.8ex; border-left: 1px solid rgb(204, 204, 204); padding-left: 1ex;">
<div>
Sorry you are right!!<br>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.<br>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?? <br>
In the function <font color="#0070c0">safety_manager->is_state_safe <font color="#000000">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</font></font> ("/map"). Has someone done this in a more efficient way??<br>
<br>Thanks !!!<br><br>Nicolas <br><br><font color="#0070c0">void Kernel::Callback(const sensor_msgs::LaserScan::ConstPtr& cloudlaser){</font><font color="#0070c0"><br></font><font color="#0070c0"> </font><font color="#0070c0"><br>
</font><font color="#0070c0"> </font><font color="#0070c0"><br></font><font color="#0070c0"> try{</font><font color="#0070c0"><br></font><font color="#0070c0"> tlistener.waitForTransform("/map", cloudlaser->header.frame_id,</font><font color="#0070c0"><br>
</font><font color="#0070c0"> cloudlaser->header.stamp, ros::Duration(1.0));</font><font color="#0070c0"><br></font><font color="#0070c0"><br></font><font color="#0070c0"> tlistener.lookupTransform("/map", cloudlaser->header.frame_id,</font><font color="#0070c0"><br>
</font><font color="#0070c0"> cloudlaser->header.stamp, lasertransform);</font><font color="#0070c0"><br></font><font color="#0070c0"> </font><font color="#0070c0"><br></font><font color="#0070c0"> </font><font color="#0070c0"><br>
</font><font color="#0070c0"> pcl::transformAsMatrix(lasertransform, mat);</font><font color="#0070c0"><br></font><font color="#0070c0"><br></font><font color="#0070c0"> }</font><font color="#0070c0"><br></font><font color="#0070c0"> catch (tf::TransformException ex){</font><font color="#0070c0"><br>
</font><font color="#0070c0"> ROS_ERROR("%s",ex.what());</font><font color="#0070c0"><br></font><font color="#0070c0"> } </font><font color="#0070c0"><br></font><font color="#0070c0"> projector_.transformLaserScanToPointCloud(cloudlaser->header.frame_id,*cloudlaser, cloud,tlistener);</font><font color="#0070c0"><br>
</font><font color="#0070c0"> sensor_msgs::convertPointCloudToPointCloud2(cloud, cloud2);</font><font color="#0070c0"><br></font><font color="#0070c0"> </font><font color="#0070c0"><br></font><font color="#0070c0"> pcl::transformPointCloud(mat, cloud2, pc_toPcl); </font><font color="#0070c0"><br>
</font><font color="#0070c0"> </font><font color="#0070c0"> </font><font color="#0070c0"><br></font><font color="#0070c0"> </font><font color="#0070c0"></font><font color="#0070c0"> pcl::fromROSMsg(pc_toPcl, laser_fchecker);</font><font color="#0070c0"><br>
</font><font color="#0070c0"> double dist_states=0,best_avg=0;</font><font color="#0070c0"><br></font><font color="#0070c0"> </font><font color="#0070c0"><br></font><font color="#0070c0"> if (start_callback) {</font><font color="#0070c0"><br>
</font><font color="#0070c0"> pose2d_t robot_pose = ros_link->get_robot_pose();</font><font color="#0070c0"><br></font><font color="#0070c0"> start_state.pose = robot_pose;</font><font color="#0070c0"><br>
</font><font color="#0070c0"> start_state.vel.front = start_state.vel.ang = 0.0;</font><font color="#0070c0"><br></font><font color="#0070c0"> actual_state=start_state;</font><font color="#0070c0"><br>
</font><font color="#0070c0"> current_act.acc.front = 0;</font><font color="#0070c0"><br></font><font color="#0070c0"> current_act.acc.ang = 0;</font><font color="#0070c0"><br></font><font color="#0070c0"> start_callback=false;</font><font color="#0070c0"><br>
</font><font color="#0070c0"> </font><font color="#0070c0"><br></font><font color="#0070c0"> }</font><font color="#0070c0"><br></font><font color="#0070c0"> </font><font color="#0070c0"><br></font><font color="#0070c0">if ((current_act.acc.front==0) && ( current_act.acc.ang==0)){</font><font color="#0070c0"><br>
</font><font color="#0070c0"> pose2d_t robot_pose = ros_link->get_robot_pose();</font><font color="#0070c0"><br></font><font color="#0070c0"> start_state.pose = robot_pose;</font><font color="#0070c0"><br>
</font><font color="#0070c0"> start_state.vel.front = start_state.vel.ang = 0.0;</font><font color="#0070c0"><br></font><font color="#0070c0"> actual_state=start_state;</font><font color="#0070c0"><br>
</font><font color="#0070c0"> current_act.acc.front = 0;</font><font color="#0070c0"><br></font><font color="#0070c0"> current_act.acc.ang = 0;</font><font color="#0070c0"><br></font><font color="#0070c0">}</font><font color="#0070c0"><br>
</font><font color="#0070c0"> </font><font color="#0070c0"><br></font><font color="#0070c0"> Action control, best_control;</font><font color="#0070c0"><br></font><font color="#0070c0"> bool aux=false;</font><font color="#0070c0"><br>
</font><font color="#0070c0"> metric_t best_result = 0; </font><font color="#0070c0"><br></font><font color="#0070c0"> </font><font color="#0070c0"><br></font><font color="#0070c0"> </font><font color="#0070c0"><br>
</font><font color="#0070c0"> for(double fv = -0.8; fv <= 0.8; fv += 0.2){ </font><font color="#0070c0"><br></font><font color="#0070c0"> for(double av =-0.75; av <= 0.75; av += 0.25){</font><font color="#0070c0"><br>
</font><font color="#0070c0"> control.acc.front = fv;</font><font color="#0070c0"><br></font><font color="#0070c0"> control.acc.ang = av;</font><font color="#0070c0"><br></font><font color="#0070c0"> </font><font color="#0070c0"><br>
</font><font color="#0070c0"> if ((control.acc.front == current_act.acc.front) && (control.acc.ang == current_act.acc.ang))</font><font color="#0070c0"><br></font><font color="#0070c0"> continue;</font><font color="#0070c0"><br>
</font><font color="#0070c0"> new_state = integrator->integrate(actual_state, control);</font><font color="#0070c0"><br></font><font color="#0070c0"> </font><font color="#0070c0"><br></font><font color="#0070c0"> if (!safety_manager->is_state_safe(actual_state, new_state, &laser_fchecker)){ // PICS checker and Mapcheker</font><font color="#0070c0"><br>
</font><font color="#0070c0"> ROS_INFO("State_No_Safe:\n"); </font><font color="#0070c0"><br></font><font color="#0070c0"> aux=false;</font><font color="#0070c0"><br></font><font color="#0070c0"> continue;</font><font color="#0070c0"><br>
</font><font color="#0070c0"> } </font><font color="#0070c0"><br></font><font color="#0070c0"> else</font><font color="#0070c0"><br></font><font color="#0070c0"> aux=true;</font><font color="#0070c0"><br>
</font><font color="#0070c0"> best_state = new_state;</font><font color="#0070c0"><br></font><font color="#0070c0"> best_control.acc.front =control.acc.front ;</font><font color="#0070c0"><br>
</font><font color="#0070c0"> best_control.acc.ang=control.acc.ang;</font><font color="#0070c0"><br></font><font color="#0070c0"> best_control.end_pose=new_state.pose;</font><font color="#0070c0"><br>
</font><font color="#0070c0"> </font><font color="#0070c0"><br></font><font color="#0070c0"> } </font><font color="#0070c0"><br></font><font color="#0070c0"> </font><font color="#0070c0"><br>
</font><font color="#0070c0"> }</font><font color="#0070c0"><br></font><font color="#0070c0"> if (!aux){</font><font color="#0070c0"><br></font><font color="#0070c0"> best_control.acc.front= best_control.acc.ang=0; </font><font color="#0070c0"><br>
</font><font color="#0070c0"> best_control.end_pose=actual_state.pose;</font><font color="#0070c0"><br></font><font color="#0070c0"> best_state=actual_state;</font><font color="#0070c0"><br>
</font><font color="#0070c0"> }</font><font color="#0070c0"><br></font><font color="#0070c0"> </font><font color="#0070c0"><br></font><font color="#0070c0"> </font><font color="#0070c0"><br></font><font color="#0070c0"> vel2d_t cmd_vel;</font><font color="#0070c0"><br>
</font><font color="#0070c0"> pose2d_t robot_pose;</font><font color="#0070c0"><br></font><font color="#0070c0"> ros::Rate loop_rate(250);</font><font color="#0070c0"><br></font><font color="#0070c0"> </font><font color="#0070c0"><br>
</font><font color="#0070c0"> current_act= best_control;</font><font color="#0070c0"><br></font><font color="#0070c0"> </font><font color="#0070c0"><br></font><font color="#0070c0"> robot_pose = ros_link->get_robot_pose();</font><font color="#0070c0"><br>
</font><font color="#0070c0"> cmd_vel = calculate_control(current_act, robot_pose);</font><font color="#0070c0"><br></font><font color="#0070c0"> ros_link->send_cmd_vel(cmd_vel);</font><font color="#0070c0"><br>
</font><font color="#0070c0"> actual_state=best_state; </font><font color="#0070c0"><br></font><font color="#0070c0"> }</font><font color="#0070c0"><br></font><br><br><br>where :<br><br><br><br><font color="#0070c0">bool Safety_Manager::is_state_safe (Phase_State state1, Phase_State state2, pcl::PointCloud<pcl::PointXYZ>* pts_laser){</font><font color="#0070c0"><br>
</font><font color="#0070c0"> </font><font color="#0070c0"><br></font><font color="#0070c0"> robot_r = 1;</font><font color="#0070c0"><br></font><font color="#0070c0"> double dist_obstacles, dist_states,dist_obstacles1;</font><font color="#0070c0"><br>
</font><font color="#0070c0"> </font><font color="#0070c0"><br></font><font color="#0070c0"> dist_states = dist2d(state1.pose, state2.pose);</font><font color="#0070c0"><br></font><font color="#0070c0"> int size_cloud=pts_laser->points.size();</font><div class="im">
<font color="#0070c0"><br></font> <font color="#0070c0"><br></font><font color="#0070c0"> for(int it=0; it<size_cloud; it++){</font><font color="#0070c0"><br></font><font color="#0070c0"> 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</font><font color="#0070c0"><br>
</font></div><font color="#0070c0"> 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</font><font color="#0070c0"><br>
</font><font color="#0070c0"> return false;</font><font color="#0070c0"><br></font><font color="#0070c0"> }</font><font color="#0070c0"><br></font><font color="#0070c0"> }</font><font color="#0070c0"><br>
</font><font color="#0070c0"> return true;</font><font color="#0070c0"><br></font><font color="#0070c0">}</font><font color="#0070c0"><br></font><br><hr>Date: Tue, 18 Jan 2011 09:33:13 -0800<br>From: <a href="mailto:bhaskara@willowgarage.com" target="_blank">bhaskara@willowgarage.com</a><br>
To: <a href="mailto:ros-users@code.ros.org" target="_blank">ros-users@code.ros.org</a><br>Subject: Re: [ros-users] Obstacles Detection with laser data<div><div></div><div class="h5"><br><br>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?<br>
- Bhaskara<br><br><div>2011/1/18 Nicolįs Alvarez Picco <span dir="ltr"><<a href="mailto:nicolasapicco@hotmail.com" target="_blank">nicolasapicco@hotmail.com</a>></span><br><blockquote style="padding-left: 1ex;">
<div>
Hi Everybody!!<br><br>I want to detect objects, but just their position to avoid them with the laser data. So I am using these lines:<br><br> for(int it=0; it<size_cloud; it++){<br> 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<br>
<br> 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<br> <br>
return false;<br> }<br> } <br><br>where pts_laser is a pcl_pointcloud and the state2 give me the future position of the robot.<br>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.<br>
This thing is not working well. So do you have a better solution or an idea??<br><br>Thanks<br><br>Nicolas<br> </div>
<br>_______________________________________________<br>
ros-users mailing list<br>
<a href="mailto:ros-users@code.ros.org" target="_blank">ros-users@code.ros.org</a><br>
<a href="https://code.ros.org/mailman/listinfo/ros-users" target="_blank">https://code.ros.org/mailman/listinfo/ros-users</a><br>
<br></blockquote></div><br><br clear="all"><br>-- <br>Bhaskara Marthi<br>Research Scientist<br>Willow Garage Inc.<br>650-475-2856<br>
<br></div></div>_______________________________________________
ros-users mailing list
<a href="mailto:ros-users@code.ros.org" target="_blank">ros-users@code.ros.org</a>
<a href="https://code.ros.org/mailman/listinfo/ros-users" target="_blank">https://code.ros.org/mailman/listinfo/ros-users</a> </div>
<br>_______________________________________________<br>
ros-users mailing list<br>
<a href="mailto:ros-users@code.ros.org">ros-users@code.ros.org</a><br>
<a href="https://code.ros.org/mailman/listinfo/ros-users" target="_blank">https://code.ros.org/mailman/listinfo/ros-users</a><br>
<br></blockquote></div><br><br clear="all"><br>-- <br>Bhaskara Marthi<br>Research Scientist<br>Willow Garage Inc.<br>650-475-2856<br>