Revision: 6357
http://playerstage.svn.sourceforge.net/playerstage/?rev=6357&view=rev
Author: gbiggs
Date: 2008-04-18 00:36:45 -0700 (Fri, 18 Apr 2008)
Log Message:
-----------
Merging changes 4451:4458 from trunk
Modified Paths:
--------------
code/player/branches/cmake/server/drivers/laser/lasercutter.cc
Modified: code/player/branches/cmake/server/drivers/laser/lasercutter.cc
===================================================================
--- code/player/branches/cmake/server/drivers/laser/lasercutter.cc
2008-04-18 07:32:41 UTC (rev 6356)
+++ code/player/branches/cmake/server/drivers/laser/lasercutter.cc
2008-04-18 07:36:45 UTC (rev 6357)
@@ -161,46 +161,39 @@
// Construct the outgoing laser packet
this->data.resolution = indata->resolution;
- this->data.min_angle = (min_angle);
- this->data.max_angle = (max_angle);
+ this->data.min_angle = min_angle;
+ this->data.max_angle = max_angle;
this->data.max_range = indata->max_range;
this->data.id = indata->id;
this->data.ranges_count = 0;
this->data.intensity_count = 0;
- current_angle = indata->min_angle;
- if (indata->ranges_count+1 > allocated_ranges)
+ // check we have space for the scans
+ if (indata->ranges_count+1 > allocated_ranges || indata->intensity_count+1 >
allocated_ranges)
{
- data.ranges =
(float*)realloc(data.ranges,sizeof(data.ranges[0])*(indata->ranges_count+1));
- data.intensity =
(uint8_t*)realloc(data.intensity,sizeof(data.intensity[0])*(indata->ranges_count+1));
+ allocated_ranges = (indata->ranges_count+1) > (indata->intensity_count+1)
? indata->ranges_count+1 : indata->intensity_count+1;
+ data.ranges =
(float*)realloc(data.ranges,sizeof(data.ranges[0])*(allocated_ranges));
+ data.intensity =
(uint8_t*)realloc(data.intensity,sizeof(data.intensity[0])*(allocated_ranges));
}
- for (i = 0; i < indata->ranges_count; i++)
+
+
+ current_angle = indata->min_angle;
+ for (i = 0; i < indata->ranges_count && current_angle <= max_angle; i++)
{
- if ((current_angle >= min_angle) && (current_angle <= max_angle))
+ if (current_angle >= min_angle)
{
this->data.ranges[this->data.ranges_count] = indata->ranges[i];
this->data.ranges_count++;
+ if (i < indata->intensity_count)
+ {
+ this->data.intensity[this->data.intensity_count] =
indata->intensity[i];
+ this->data.intensity_count++;
+ }
}
current_angle += indata->resolution;
}
- current_angle = indata->min_angle;
- if (indata->intensity_count+1 > allocated_ranges)
- {
- data.ranges =
(float*)realloc(data.ranges,sizeof(data.ranges[0])*(indata->intensity_count+1));
- data.intensity =
(uint8_t*)realloc(data.intensity,sizeof(data.intensity[0])*(indata->intensity_count+1));
- }
- for (i = 0; i < indata->intensity_count; i++)
- {
- if ((current_angle >= min_angle) && (current_angle <= max_angle))
- {
- this->data.intensity[this->data.intensity_count] = indata->intensity[i];
- this->data.intensity_count++;
- }
- current_angle += indata->resolution;
- }
-
this->Publish(this->device_addr, PLAYER_MSGTYPE_DATA,
PLAYER_LASER_DATA_SCAN, &this->data);
return 1;
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
This SF.net email is sponsored by the 2008 JavaOne(SM) Conference
Don't miss this year's exciting event. There's still time to save $100.
Use priority code J8TL2D2.
http://ad.doubleclick.net/clk;198757673;13503038;p?http://java.sun.com/javaone
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
