Re: [Playerstage-users] Using VFH with sonar

[Brian G. <br...@ge...>]

On Jul 31, 2008, at 1:26 AM, Rastaxe wrote:

> Hi, I want to use the VFH algorithm with sonar. I never used laser,  
> so I don't know how it works and how is its output.
> In the code of the VFH I see this block of codes that convert sonar  
> data in laser data:
>
> for(i = 0; i < (int)data.ranges_count; i++)
>   {
>     for(b = RTOD(this->sonar_poses[i].pa) + 90.0 - cone_width/2.0;
>         b < RTOD(this->sonar_poses[i].pa) + 90.0 + cone_width/2.0;
>         b+=0.5)
>     {
>       if((b < 0) || (rint(b*2) >= count))
>         continue;
>
>       sonarDistToCenter = sqrt(pow(this->sonar_poses[i].px,2) +  
> pow(this->sonar_poses[i].py,2));//0.32
>       this->laser_ranges[(int)rint(b * 2)][0] = (sonarDistToCenter +  
> data.ranges[i]) * 1e3;//b*2 = 290
>       this->laser_ranges[(int)rint(b * 2)][1] = b;
>     }
>   }
>
> So the important thing is sonar_poses.pa of every sonars. In my  
> robot I have 4 sonar in front of it with this poses
> sonar_poses[0].pa = DTOR(50);
> sonar_poses[1].pa = DTOR(20);
> sonar_poses[2].pa = DTOR(-20);
> sonar_poses[3].pa = DTOR(-50);
>
> Is it right? Or I have to use only positive numbers? With this  
> values the turn value output of the VFH doesn't seem influenced
> from the sonar data 2 and 3. Some advice?

The negative numbers should be fine.  Are you also setting the .px  
and .py values for the sonar poses?

	brian.