Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Lidarimprovements #2011

Merged
merged 1 commit into from
Sep 13, 2019
Merged

Lidarimprovements #2011

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions AirLib/include/api/RpcLibClientBase.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,9 @@ class RpcLibClientBase {
msr::airlib::GpsBase::Output getGpsData(const std::string& gps_name = "", const std::string& vehicle_name = "") const;
msr::airlib::DistanceBase::Output getDistanceSensorData(const std::string& distance_sensor_name = "", const std::string& vehicle_name = "") const;

// sensor omniscient APIs
vector<int> simGetLidarSegmentation(const std::string& lidar_name = "", const std::string& vehicle_name = "") const;

Pose simGetVehiclePose(const std::string& vehicle_name = "") const;
void simSetVehiclePose(const Pose& pose, bool ignore_collision, const std::string& vehicle_name = "");

Expand Down
45 changes: 31 additions & 14 deletions AirLib/include/api/VehicleApiBase.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -110,26 +110,22 @@ class VehicleApiBase : public UpdatableObject {
// Lidar APIs
virtual LidarData getLidarData(const std::string& lidar_name) const
{
const LidarBase* lidar = nullptr;

// Find lidar with the given name (for empty input name, return the first one found)
// Not efficient but should suffice given small number of lidars
uint count_lidars = getSensors().size(SensorBase::SensorType::Lidar);
for (uint i = 0; i < count_lidars; i++)
{
const LidarBase* current_lidar = static_cast<const LidarBase*>(getSensors().getByType(SensorBase::SensorType::Lidar, i));
if (current_lidar != nullptr && (current_lidar->getName() == lidar_name || lidar_name == ""))
{
lidar = current_lidar;
break;
}
}
auto *lidar = findLidarByName(lidar_name);
if (lidar == nullptr)
throw VehicleControllerException(Utils::stringf("No lidar with name %s exist on vehicle", lidar_name.c_str()));

return lidar->getOutput();
}

virtual vector<int> getLidarSegmentation(const std::string& lidar_name) const
{
auto *lidar = findLidarByName(lidar_name);
if (lidar == nullptr)
throw VehicleControllerException(Utils::stringf("No lidar with name %s exist on vehicle", lidar_name.c_str()));

return lidar->getSegmentationOutput();
}

// IMU API
virtual ImuBase::Output getImuData(const std::string& imu_name) const
{
Expand Down Expand Up @@ -260,6 +256,27 @@ class VehicleApiBase : public UpdatableObject {
: VehicleControllerException(message) {
}
};

private:
const LidarBase* findLidarByName(const std::string& lidar_name) const
{
const LidarBase* lidar = nullptr;

// Find lidar with the given name (for empty input name, return the first one found)
// Not efficient but should suffice given small number of lidars
uint count_lidars = getSensors().size(SensorBase::SensorType::Lidar);
for (uint i = 0; i < count_lidars; i++)
{
const LidarBase* current_lidar = static_cast<const LidarBase*>(getSensors().getByType(SensorBase::SensorType::Lidar, i));
if (current_lidar != nullptr && (current_lidar->getName() == lidar_name || lidar_name == ""))
{
lidar = current_lidar;
break;
}
}

return lidar;
}
};


Expand Down
11 changes: 11 additions & 0 deletions AirLib/include/sensors/lidar/LidarBase.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,14 +45,25 @@ class LidarBase : public SensorBase {
return output_;
}

const vector<int>& getSegmentationOutput() const
{
return segmentation_output_;
}

protected:
void setOutput(const LidarData& output)
{
output_ = output;
}

void setSegmentationOutput(vector<int>& segmentation_output)
{
segmentation_output_ = segmentation_output;
}

private:
LidarData output_;
vector<int> segmentation_output_;
};

}} //namespace
Expand Down
10 changes: 7 additions & 3 deletions AirLib/include/sensors/lidar/LidarSimple.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ class LidarSimple : public LidarBase {

protected:
virtual void getPointCloud(const Pose& lidar_pose, const Pose& vehicle_pose,
TTimeDelta delta_time, vector<real_T>& point_cloud) = 0;
TTimeDelta delta_time, vector<real_T>& point_cloud, vector<int>& segmentation_cloud) = 0;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Does it make sense to have a different sensor - LidarSimpleSegmentation which has the segmentation IDs per point?
I am not sure how much is the effect of getting segmentation IDs is on a pure geometric lidar in the backend, as the end user may / may not need getSegmentationOutput

If you have a different sensor, you can have getLidarSegmentationData which can a return a object of type msr::airlib::LidarDatawithSegmentation (or a innocuously misleading msr::airlib::LidarSegmentationData), which contains XYZSegID

latter seems a more cleaner API to me



private: //methods
Expand All @@ -91,8 +91,10 @@ class LidarSimple : public LidarBase {
Pose lidar_pose = params_.relative_pose + ground_truth.kinematics->pose;
getPointCloud(params_.relative_pose, // relative lidar pose
ground_truth.kinematics->pose, // relative vehicle pose
delta_time,
point_cloud_);
delta_time,
point_cloud_,
segmentation_cloud_
);

LidarData output;
output.point_cloud = point_cloud_;
Expand All @@ -102,11 +104,13 @@ class LidarSimple : public LidarBase {
last_time_ = output.time_stamp;

setOutput(output);
setSegmentationOutput(segmentation_cloud_);
}

private:
LidarSimpleParams params_;
vector<real_T> point_cloud_;
vector<int> segmentation_cloud_;

FrequencyLimiter freq_limiter_;
TTimePoint last_time_;
Expand Down
7 changes: 6 additions & 1 deletion AirLib/src/api/RpcLibClientBase.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,7 @@ int RpcLibClientBase::getMinRequiredClientVersion() const
}
int RpcLibClientBase::getServerVersion() const
{
return pimpl_->client.call("getServerVersion").as<int>();
return pimpl_->client.call("getServerVersion").as<int>();
}

void RpcLibClientBase::reset()
Expand Down Expand Up @@ -189,6 +189,11 @@ msr::airlib::DistanceBase::Output RpcLibClientBase::getDistanceSensorData(const
return pimpl_->client.call("getDistanceSensorData", distance_sensor_name, vehicle_name).as<RpcLibAdapatorsBase::DistanceSensorData>().to();
}

vector<int> RpcLibClientBase::simGetLidarSegmentation(const std::string& lidar_name, const std::string& vehicle_name) const
{
return pimpl_->client.call("simGetLidarSegmentation", lidar_name, vehicle_name).as<vector<int>>();
}

bool RpcLibClientBase::simSetSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex)
{
return pimpl_->client.call("simSetSegmentationObjectID", mesh_name, object_id, is_name_regex).as<bool>();
Expand Down
5 changes: 5 additions & 0 deletions AirLib/src/api/RpcLibServerBase.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -126,6 +126,11 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
return RpcLibAdapatorsBase::Pose(pose);
});

pimpl_->server.
bind("simGetLidarSegmentation", [&](const std::string& lidar_name, const std::string& vehicle_name) -> std::vector<int> {
return getVehicleApi(vehicle_name)->getLidarSegmentation(lidar_name);
});

pimpl_->server.
bind("simSetSegmentationObjectID", [&](const std::string& mesh_name, int object_id, bool is_name_regex) -> bool {
return getWorldSimApi()->setSegmentationObjectID(mesh_name, object_id, is_name_regex);
Expand Down
3 changes: 3 additions & 0 deletions PythonClient/airsim/client.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -161,6 +161,9 @@ def getDistanceSensorData(self, lidar_name = '', vehicle_name = ''):

def getLidarData(self, lidar_name = '', vehicle_name = ''):
return LidarData.from_msgpack(self.client.call('getLidarData', lidar_name, vehicle_name))

def simGetLidarSegmentation(self, lidar_name = '', vehicle_name = ''):
return self.client.call('simGetLidarSegmentation', lidar_name, vehicle_name)

#----------- APIs to control ACharacter in scene ----------/
def simCharSetFaceExpression(self, expression_name, value, character_name = ""):
Expand Down
21 changes: 18 additions & 3 deletions Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,9 +42,10 @@ void UnrealLidarSensor::createLasers()

// returns a point-cloud for the tick
void UnrealLidarSensor::getPointCloud(const msr::airlib::Pose& lidar_pose, const msr::airlib::Pose& vehicle_pose,
const msr::airlib::TTimeDelta delta_time, msr::airlib::vector<msr::airlib::real_T>& point_cloud)
const msr::airlib::TTimeDelta delta_time, msr::airlib::vector<msr::airlib::real_T>& point_cloud, vector<int>& segmentation_cloud)
{
point_cloud.clear();
segmentation_cloud.clear();

msr::airlib::LidarSimpleParams params = getParams();
const auto number_of_lasers = params.number_of_channels;
Expand Down Expand Up @@ -89,12 +90,14 @@ void UnrealLidarSensor::getPointCloud(const msr::airlib::Pose& lidar_pose, const
continue;

Vector3r point;
int segmentationID = -1;
// shoot laser and get the impact point, if any
if (shootLaser(lidar_pose, vehicle_pose, laser, horizontal_angle, vertical_angle, params, point))
if (shootLaser(lidar_pose, vehicle_pose, laser, horizontal_angle, vertical_angle, params, point, segmentationID))
{
point_cloud.emplace_back(point.x());
point_cloud.emplace_back(point.y());
point_cloud.emplace_back(point.z());
segmentation_cloud.emplace_back(segmentationID);
}
}
}
Expand All @@ -107,7 +110,7 @@ void UnrealLidarSensor::getPointCloud(const msr::airlib::Pose& lidar_pose, const
// simulate shooting a laser via Unreal ray-tracing.
bool UnrealLidarSensor::shootLaser(const msr::airlib::Pose& lidar_pose, const msr::airlib::Pose& vehicle_pose,
const uint32 laser, const float horizontal_angle, const float vertical_angle,
const msr::airlib::LidarSimpleParams params, Vector3r &point)
const msr::airlib::LidarSimpleParams params, Vector3r &point, int &segmentationID)
{
// start position
Vector3r start = lidar_pose.position + vehicle_pose.position;
Expand Down Expand Up @@ -135,6 +138,18 @@ bool UnrealLidarSensor::shootLaser(const msr::airlib::Pose& lidar_pose, const ms

if (is_hit)
{
//Store the segmentation id of the hit object.
auto hitActor = hit_result.GetActor();
if (hitActor != nullptr)
{
TArray<UMeshComponent*> meshComponents;
hitActor->GetComponents<UMeshComponent>(meshComponents);
for (int i = 0; i < meshComponents.Num(); i++)
{
segmentationID = segmentationID == -1 ? meshComponents[i]->CustomDepthStencilValue : segmentationID;
}
}

if (false && UAirBlueprintLib::IsInGameThread())
{
// Debug code for very specific cases.
Expand Down
4 changes: 2 additions & 2 deletions Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealLidarSensor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ class UnrealLidarSensor : public msr::airlib::LidarSimple {

protected:
virtual void getPointCloud(const msr::airlib::Pose& lidar_pose, const msr::airlib::Pose& vehicle_pose,
msr::airlib::TTimeDelta delta_time, msr::airlib::vector<msr::airlib::real_T>& point_cloud) override;
msr::airlib::TTimeDelta delta_time, msr::airlib::vector<msr::airlib::real_T>& point_cloud, vector<int>& segmentation_cloud) override;

private:
using Vector3r = msr::airlib::Vector3r;
Expand All @@ -30,7 +30,7 @@ class UnrealLidarSensor : public msr::airlib::LidarSimple {
void createLasers();
bool shootLaser(const msr::airlib::Pose& lidar_pose, const msr::airlib::Pose& vehicle_pose,
const uint32 channel, const float horizontal_angle, const float vertical_angle,
const msr::airlib::LidarSimpleParams params, Vector3r &point);
const msr::airlib::LidarSimpleParams params, Vector3r &point, int &segmentationID);

private:
AActor* actor_;
Expand Down
2 changes: 2 additions & 0 deletions UnrealPluginFiles.vcxproj
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,6 +95,7 @@
<ClCompile Include="unreal\plugins\airsim\source\simmode\SimModeWorldBase.cpp" />
<ClCompile Include="Unreal\Plugins\AirSim\Source\UnrealImageCapture.cpp" />
<ClCompile Include="Unreal\Plugins\AirSim\Source\UnrealSensors\UnrealDistanceSensor.cpp" />
<ClCompile Include="Unreal\Plugins\AirSim\Source\UnrealSensors\UnrealLidarSensor.cpp" />
<ClCompile Include="Unreal\Plugins\AirSim\Source\UnrealSensors\UnrealSensorFactory.cpp" />
<ClCompile Include="unreal\plugins\airsim\source\VehicleCameraConnector.cpp" />
<ClCompile Include="Unreal\Plugins\AirSim\Source\VehicleSimApi.cpp" />
Expand Down Expand Up @@ -128,6 +129,7 @@
<ClInclude Include="unreal\plugins\airsim\source\simmode\SimModeWorldBase.h" />
<ClInclude Include="Unreal\Plugins\AirSim\Source\UnrealImageCapture.h" />
<ClInclude Include="Unreal\Plugins\AirSim\Source\UnrealSensors\UnrealDistanceSensor.h" />
<ClInclude Include="Unreal\Plugins\AirSim\Source\UnrealSensors\UnrealLidarSensor.h" />
<ClInclude Include="Unreal\Plugins\AirSim\Source\UnrealSensors\UnrealSensorFactory.h" />
<ClInclude Include="unreal\plugins\airsim\source\VehicleCameraConnector.h" />
<ClInclude Include="unreal\plugins\airsim\source\VehiclePawnWrapper.h" />
Expand Down
6 changes: 6 additions & 0 deletions UnrealPluginFiles.vcxproj.filters
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -105,6 +105,9 @@
<ClCompile Include="Unreal\Plugins\AirSim\Source\Weather\WeatherLib.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="Unreal\Plugins\AirSim\Source\UnrealSensors\UnrealLidarSensor.cpp">
<Filter>Source Files</Filter>
</ClCompile>
</ItemGroup>
<ItemGroup>
<ClInclude Include="unreal\plugins\airsim\source\AirBlueprintLib.h">
Expand Down Expand Up @@ -203,6 +206,9 @@
<ClInclude Include="Unreal\Plugins\AirSim\Source\Weather\WeatherLib.h">
<Filter>Source Files</Filter>
</ClInclude>
<ClInclude Include="Unreal\Plugins\AirSim\Source\UnrealSensors\UnrealLidarSensor.h">
<Filter>Source Files</Filter>
</ClInclude>
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

kinda surprised that this was already not in the upstream vcxproj

</ItemGroup>
<ItemGroup>
<None Include="Unreal\Plugins\AirSim\Source\AirSim.Build.4.15.cs">
Expand Down