<html>
<head>
<style><!--
.hmmessage P
{
margin:0px;
padding:0px
}
body.hmmessage
{
font-size: 10pt;
font-family:Tahoma
}
--></style>
</head>
<body class='hmmessage'>
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 style="" color="#0070c0">safety_manager->is_state_safe <font style="" 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 style="" color="#0070c0">void Kernel::Callback(const sensor_msgs::LaserScan::ConstPtr& cloudlaser){</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> try{</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> tlistener.waitForTransform("/map", cloudlaser->header.frame_id,</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> cloudlaser->header.stamp, ros::Duration(1.0));</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> tlistener.lookupTransform("/map", cloudlaser->header.frame_id,</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> cloudlaser->header.stamp, lasertransform);</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> pcl::transformAsMatrix(lasertransform, mat);</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> }</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> catch (tf::TransformException ex){</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> ROS_ERROR("%s",ex.what());</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> } </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> projector_.transformLaserScanToPointCloud(cloudlaser->header.frame_id,*cloudlaser, cloud,tlistener);</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> sensor_msgs::convertPointCloudToPointCloud2(cloud, cloud2);</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> pcl::transformPointCloud(mat, cloud2, pc_toPcl); </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"></font><font style="" color="#0070c0"> pcl::fromROSMsg(pc_toPcl, laser_fchecker);</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> double dist_states=0,best_avg=0;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> if (start_callback) {</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> pose2d_t robot_pose = ros_link->get_robot_pose();</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> start_state.pose = robot_pose;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> start_state.vel.front = start_state.vel.ang = 0.0;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> actual_state=start_state;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> current_act.acc.front = 0;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> current_act.acc.ang = 0;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> start_callback=false;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> }</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0">if ((current_act.acc.front==0) && ( current_act.acc.ang==0)){</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> pose2d_t robot_pose = ros_link->get_robot_pose();</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> start_state.pose = robot_pose;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> start_state.vel.front = start_state.vel.ang = 0.0;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> actual_state=start_state;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> current_act.acc.front = 0;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> current_act.acc.ang = 0;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0">}</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> Action control, best_control;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> bool aux=false;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> metric_t best_result = 0; </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> for(double fv = -0.8; fv <= 0.8; fv += 0.2){ </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> for(double av =-0.75; av <= 0.75; av += 0.25){</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> control.acc.front = fv;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> control.acc.ang = av;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> if ((control.acc.front == current_act.acc.front) && (control.acc.ang == current_act.acc.ang))</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> continue;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> new_state = integrator->integrate(actual_state, control);</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> if (!safety_manager->is_state_safe(actual_state, new_state, &laser_fchecker)){ // PICS checker and Mapcheker</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> ROS_INFO("State_No_Safe:\n"); </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> aux=false;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> continue;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> } </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> else</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> aux=true;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> best_state = new_state;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> best_control.acc.front =control.acc.front ;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> best_control.acc.ang=control.acc.ang;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> best_control.end_pose=new_state.pose;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> } </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> }</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> if (!aux){</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> best_control.acc.front= best_control.acc.ang=0; </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> best_control.end_pose=actual_state.pose;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> best_state=actual_state;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> }</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> vel2d_t cmd_vel;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> pose2d_t robot_pose;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> ros::Rate loop_rate(250);</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> current_act= best_control;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> robot_pose = ros_link->get_robot_pose();</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> cmd_vel = calculate_control(current_act, robot_pose);</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> ros_link->send_cmd_vel(cmd_vel);</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> actual_state=best_state; </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> }</font><font style="" color="#0070c0"><br></font><br><br><br>where :<br><br><br><br><font style="" color="#0070c0">bool Safety_Manager::is_state_safe (Phase_State state1, Phase_State state2, pcl::PointCloud<pcl::PointXYZ>* pts_laser){</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> robot_r = 1;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> double dist_obstacles, dist_states,dist_obstacles1;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> </font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> dist_states = dist2d(state1.pose, state2.pose);</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> int size_cloud=pts_laser->points.size();</font><font style="" color="#0070c0"><br></font> <font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> for(int it=0; it<size_cloud; it++){</font><font style="" color="#0070c0"><br></font><font style="" 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 style="" color="#0070c0"><br></font><font style="" 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 style="" color="#0070c0"><br></font><font style="" color="#0070c0"> return false;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> }</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> }</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0"> return true;</font><font style="" color="#0070c0"><br></font><font style="" color="#0070c0">}</font><font style="" color="#0070c0"><br></font><br><hr id="stopSpelling">Date: Tue, 18 Jan 2011 09:33:13 -0800<br>From: bhaskara@willowgarage.com<br>To: ros-users@code.ros.org<br>Subject: Re: [ros-users] Obstacles Detection with laser data<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 class="ecxgmail_quote">2011/1/18 Nicolás Alvarez Picco <span dir="ltr"><<a href="mailto:nicolasapicco@hotmail.com">nicolasapicco@hotmail.com</a>></span><br><blockquote class="ecxgmail_quote" 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">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>_______________________________________________
ros-users mailing list
ros-users@code.ros.org
https://code.ros.org/mailman/listinfo/ros-users </body>
</html>