visualization_msgs::MarkerArray
VisualizeDetectedObjects::ObjectsToCentroids(const autoware_msgs::DetectedObjectArray &in_objects)
{
visualization_msgs::MarkerArray centroid_markers;
for (auto const &object: in_objects.objects)
{
if (IsObjectValid(object))
{
visualization_msgs::Marker centroid_marker;
centroid_marker.lifetime = ros::Duration(marker_display_duration_);
centroid_marker.header = in_objects.header;
centroid_marker.type = visualization_msgs::Marker::SPHERE;
centroid_marker.action = visualization_msgs::Marker::ADD;
centroid_marker.pose = object.pose;
centroid_marker.ns = ros_namespace_ + "/centroid_markers";
centroid_marker.scale.x = 0.5;
centroid_marker.scale.y = 0.5;
centroid_marker.scale.z = 0.5;
if (object.color.a == 0)
{
centroid_marker.color = centroid_color_;
}
else
{
centroid_marker.color = object.color;
}
centroid_marker.id = marker_id_++;
centroid_markers.markers.push_back(centroid_marker);
}
}
return centroid_markers;
}//ObjectsToCentroids
NTSTATUS
HOOKPORT_CALLTYPE
HookNtSetInformationThread(
PSYSCALL_INFO SysCallInfo,
PSYSTEM_CALL_ACTION Action,
HANDLE ThreadHandle,
THREAD_INFORMATION_CLASS ThreadInformationClass,
PVOID ThreadInformation,
ULONG ThreadInformationLength
)
{
NTSTATUS Status;
switch (ThreadInformationClass)
{
case ThreadHideFromDebugger:
Status = IsObjectValid(ThreadHandle);
if (NT_SUCCESS(Status))
break;
return Status;
default:
return STATUS_SUCCESS;
}
Action->Action = BlockSystemCall;
return Status;
}
visualization_msgs::MarkerArray
VisualizeDetectedObjects::ObjectsToModels(const autoware_msgs::DetectedObjectArray &in_objects)
{
visualization_msgs::MarkerArray object_models;
for (auto const &object: in_objects.objects)
{
if (IsObjectValid(object) &&
object.label != "unknown" &&
(object.dimensions.x + object.dimensions.y + object.dimensions.z) < object_max_linear_size_)
{
visualization_msgs::Marker model;
model.lifetime = ros::Duration(marker_display_duration_);
model.header = in_objects.header;
model.type = visualization_msgs::Marker::MESH_RESOURCE;
model.action = visualization_msgs::Marker::ADD;
model.ns = ros_namespace_ + "/model_markers";
model.mesh_use_embedded_materials = false;
model.color = model_color_;
if(object.label == "car")
{
model.mesh_resource = "package://detected_objects_visualizer/models/car.dae";
}
else if (object.label == "person")
{
model.mesh_resource = "package://detected_objects_visualizer/models/person.dae";
}
else if (object.label == "bicycle" || object.label == "bike")
{
model.mesh_resource = "package://detected_objects_visualizer/models/bike.dae";
}
else if (object.label == "bus")
{
model.mesh_resource = "package://detected_objects_visualizer/models/bus.dae";
}
else if(object.label == "truck")
{
model.mesh_resource = "package://detected_objects_visualizer/models/truck.dae";
}
else
{
model.mesh_resource = "package://detected_objects_visualizer/models/box.dae";
}
model.scale.x = 1;
model.scale.y = 1;
model.scale.z = 1;
model.id = marker_id_++;
model.pose.position = object.pose.position;
model.pose.position.z-= object.dimensions.z/2;
if (object.pose_reliable)
model.pose.orientation = object.pose.orientation;
object_models.markers.push_back(model);
}
}
return object_models;
}//ObjectsToModels
NTSTATUS
HOOKPORT_CALLTYPE
HookNtClose(
PSYSCALL_INFO SysCallInfo,
PSYSTEM_CALL_ACTION Action,
HANDLE Handle
)
{
NTSTATUS Status;
Status = IsObjectValid(Handle);
if (!NT_SUCCESS(Status))
Action->Action = BlockSystemCall;
return Status;
}
visualization_msgs::MarkerArray
VisualizeDetectedObjects::ObjectsToHulls(const autoware_msgs::DetectedObjectArray &in_objects)
{
visualization_msgs::MarkerArray polygon_hulls;
for (auto const &object: in_objects.objects)
{
if (IsObjectValid(object) && !object.convex_hull.polygon.points.empty() && object.label == "unknown")
{
visualization_msgs::Marker hull;
hull.lifetime = ros::Duration(marker_display_duration_);
hull.header = in_objects.header;
hull.type = visualization_msgs::Marker::LINE_STRIP;
hull.action = visualization_msgs::Marker::ADD;
hull.ns = ros_namespace_ + "/hull_markers";
hull.id = marker_id_++;
hull.scale.x = 0.2;
for(auto const &point: object.convex_hull.polygon.points)
{
geometry_msgs::Point tmp_point;
tmp_point.x = point.x;
tmp_point.y = point.y;
tmp_point.z = point.z;
hull.points.push_back(tmp_point);
}
if (object.color.a == 0)
{
hull.color = hull_color_;
}
else
{
hull.color = object.color;
}
polygon_hulls.markers.push_back(hull);
}
}
return polygon_hulls;
}
visualization_msgs::MarkerArray
VisualizeDetectedObjects::ObjectsToBoxes(const autoware_msgs::DetectedObjectArray &in_objects)
{
visualization_msgs::MarkerArray object_boxes;
for (auto const &object: in_objects.objects)
{
if (IsObjectValid(object) &&
object.label != "unknown" &&
(object.dimensions.x + object.dimensions.y + object.dimensions.z) < object_max_linear_size_)
{
visualization_msgs::Marker box;
box.lifetime = ros::Duration(marker_display_duration_);
box.header = in_objects.header;
box.type = visualization_msgs::Marker::CUBE;
box.action = visualization_msgs::Marker::ADD;
box.ns = ros_namespace_ + "/box_markers";
box.id = marker_id_++;
box.scale = object.dimensions;
box.pose.position = object.pose.position;
if (object.pose_reliable)
box.pose.orientation = object.pose.orientation;
if (object.color.a == 0)
{
box.color = box_color_;
}
else
{
box.color = object.color;
}
object_boxes.markers.push_back(box);
}
}
return object_boxes;
}//ObjectsToBoxes
visualization_msgs::MarkerArray
VisualizeDetectedObjects::ObjectsToLabels(const autoware_msgs::DetectedObjectArray &in_objects)
{
visualization_msgs::MarkerArray label_markers;
for (auto const &object: in_objects.objects)
{
if (IsObjectValid(object))
{
visualization_msgs::Marker label_marker;
label_marker.lifetime = ros::Duration(marker_display_duration_);
label_marker.header = in_objects.header;
label_marker.ns = ros_namespace_ + "/label_markers";
label_marker.action = visualization_msgs::Marker::ADD;
label_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
label_marker.scale.x = 1.5;
label_marker.scale.y = 1.5;
label_marker.scale.z = 1.5;
label_marker.color = label_color_;
label_marker.id = marker_id_++;
if(!object.label.empty() && object.label != "unknown")
label_marker.text = object.label + " "; //Object Class if available
std::stringstream distance_stream;
distance_stream << std::fixed << std::setprecision(1)
<< sqrt((object.pose.position.x * object.pose.position.x) +
(object.pose.position.y * object.pose.position.y));
std::string distance_str = distance_stream.str() + " m";
label_marker.text += distance_str;
if (object.velocity_reliable)
{
double velocity = object.velocity.linear.x;
if (velocity < -0.1)
{
velocity *= -1;
}
if (abs(velocity) < object_speed_threshold_)
{
velocity = 0.0;
}
tf::Quaternion q(object.pose.orientation.x, object.pose.orientation.y,
object.pose.orientation.z, object.pose.orientation.w);
double roll, pitch, yaw;
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
// convert m/s to km/h
std::stringstream kmh_velocity_stream;
kmh_velocity_stream << std::fixed << std::setprecision(1) << (velocity * 3.6);
std::string text = "\n<" + std::to_string(object.id) + "> " + kmh_velocity_stream.str() + " km/h";
label_marker.text += text;
}
label_marker.pose.position.x = object.pose.position.x;
label_marker.pose.position.y = object.pose.position.y;
label_marker.pose.position.z = label_height_;
label_marker.scale.z = 1.0;
if (!label_marker.text.empty())
label_markers.markers.push_back(label_marker);
}
} // end in_objects.objects loop
return label_markers;
}//ObjectsToLabels
visualization_msgs::MarkerArray
VisualizeDetectedObjects::ObjectsToArrows(const autoware_msgs::DetectedObjectArray &in_objects)
{
visualization_msgs::MarkerArray arrow_markers;
for (auto const &object: in_objects.objects)
{
if (IsObjectValid(object) && object.pose_reliable)
{
double velocity = object.velocity.linear.x;
if (abs(velocity) >= arrow_speed_threshold_)
{
visualization_msgs::Marker arrow_marker;
arrow_marker.lifetime = ros::Duration(marker_display_duration_);
tf::Quaternion q(object.pose.orientation.x,
object.pose.orientation.y,
object.pose.orientation.z,
object.pose.orientation.w);
double roll, pitch, yaw;
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
// in the case motion model fit opposite direction
if (velocity < -0.1)
{
yaw += M_PI;
// normalize angle
while (yaw > M_PI)
yaw -= 2. * M_PI;
while (yaw < -M_PI)
yaw += 2. * M_PI;
}
tf::Matrix3x3 obs_mat;
tf::Quaternion q_tf;
obs_mat.setEulerYPR(yaw, 0, 0); // yaw, pitch, roll
obs_mat.getRotation(q_tf);
arrow_marker.header = in_objects.header;
arrow_marker.ns = ros_namespace_ + "/arrow_markers";
arrow_marker.action = visualization_msgs::Marker::ADD;
arrow_marker.type = visualization_msgs::Marker::ARROW;
// green
if (object.color.a == 0)
{
arrow_marker.color = arrow_color_;
}
else
{
arrow_marker.color = object.color;
}
arrow_marker.id = marker_id_++;
// Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header
arrow_marker.pose.position.x = object.pose.position.x;
arrow_marker.pose.position.y = object.pose.position.y;
arrow_marker.pose.position.z = arrow_height_;
arrow_marker.pose.orientation.x = q_tf.getX();
arrow_marker.pose.orientation.y = q_tf.getY();
arrow_marker.pose.orientation.z = q_tf.getZ();
arrow_marker.pose.orientation.w = q_tf.getW();
// Set the scale of the arrow -- 1x1x1 here means 1m on a side
arrow_marker.scale.x = 3;
arrow_marker.scale.y = 0.1;
arrow_marker.scale.z = 0.1;
arrow_markers.markers.push_back(arrow_marker);
}//velocity threshold
}//valid object
}//end for
return arrow_markers;
}//ObjectsToArrows
void CVisDemoSorterClassifier::DoProcessing()
{
Uint32 i;
m_nNumObjectsClassifier_last = 0;
if ( m_pModel == NULL )
return;
FastLabelObject * objLabels = (FastLabelObject*)m_iportLabelData.GetBuffer();
ColorObject * objColors = (ColorObject*)m_iportColorData.GetBuffer();
// Get extents of valid area
CVisFixpoint fLeft, fRight, fTop, fBottom;
m_pModel->GetValidArea( fLeft, fRight, fTop, fBottom );
for ( i=1; i<objLabels[0].unNumObjects; i++)
{
// Clear the object's flags.
objLabels[i].unFlags = 0;
// See if the object is valid (i.e. it is in a valid position and has sensibel
// sizes.
if ( IsObjectValid( objLabels[i] ) )
{
// The object is valid.
m_nNumObjectsClassified++;
m_nNumObjectsClassifier_last++;
m_nNumFramesWithoutObjects = 0;
// Classify the object.
Int nClass = ClassifiyColor( objColors[i].Color.unHue, objColors[i].Color.unSat, objColors[i].Color.unLum );
// Increment count for that class.
if ( nClass != -1 )
m_aryColorClasses[nClass].nCount++;
// Mark the object as due for ejection if the class is valid and scheduled for ejection.
if ( (nClass != -1) && ( m_aryColorClasses[nClass].nEject != 0 ) )
objLabels[i].unFlags |= FLF_EJECT;
else
objLabels[i].unFlags &= ~FLF_EJECT;
} // if object valid
} // for all objects
// Resolve critical ejections
//ResolveCriticalEjections( objLabels );
// Generate ejection commands
for ( i=1; i<objLabels[0].unNumObjects; i++)
{
if ( (objLabels[i].unFlags & FLF_EJECT) != 0 )
EjectObject( objLabels[i] );
}
#ifndef _WINDOWS
// See if we've seen no objects for a while and turn of the jets if that's the case
Uint32 unTime = timeGetHighResTime() + timeFromMs( 10 );
if ( m_nNumFramesWithoutObjects > 25 )
{
for ( int i=0; i<m_pModel->GetNumJets(); i++ )
COutputDispatcher::Instance()->Channel( i ).AddCommand( unTime , false );
}
#endif
}
NTSTATUS
HOOKPORT_CALLTYPE
HookNtQueryInformationProcess(
PSYSCALL_INFO SysCallInfo,
PSYSTEM_CALL_ACTION Action,
HANDLE ProcessHandle,
PROCESS_INFORMATION_CLASS ProcessInformationClass,
PVOID ProcessInformation,
ULONG ProcessInformationLength,
PULONG ReturnLength
)
{
NTSTATUS Status;
/*
static ULONG Count;
PrintConsoleW(
L"%08X: %S\n"
L"ProcessInformationClass = %08d\n"
L"ProcessInformation = %08X\n"
L"ProcessInformationLength = %08X\n"
L"ReturnLength = %08X\n"
L"\n",
Count++,
g_pSysCallFilters[ServiceIndex].Reserve,
ProcessInformationClass,
ProcessInformation,
ProcessInformationLength,
ReturnLength
);
*/
Status = IsObjectValid(ProcessHandle);
if (!NT_SUCCESS(Status))
goto _Exit;
switch (ProcessInformationClass)
{
case ProcessDebugObjectHandle:
Status = STATUS_PORT_NOT_SET;
case ProcessDebugPort:
if (ProcessInformation != NULL)
{
ZeroMemory(ProcessInformation, ProcessInformationLength);
}
if (ReturnLength != NULL)
{
*ReturnLength = 4;
}
break;
/*
case ProcessDebugFlags:
if (ProcessInformation != 0)
*(PULONG)ProcessInformation = TRUE;
if (ReturnLength != NULL)
*ReturnLength = 4;
break;
*/
/*
case ProcessBasicInformation:
if (ProcessHandle != NtCurrentProcess())
goto _Exit;
Status = CallSysCall(
NtQueryInformationProcess,
SysCallInfo,
ProcessHandle,
ProcessInformationClass,
ProcessInformation,
ProcessInformationLength,
ReturnLength
);
if (NT_SUCCESS(Status))
((PPROCESS_BASIC_INFORMATION)ProcessInformation)->InheritedFromUniqueProcessId = g_ExplorerPID;
break;
*/
default:
goto _Exit;
}
Action->Action = BlockSystemCall;
_Exit:
return Status;
}
