diff --git a/aloha-devel/act/__pycache__/policy.cpython-38.pyc b/aloha-devel/act/__pycache__/policy.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..e6e535996cb7ead348e139d6fbc0456810b6af65 Binary files /dev/null and b/aloha-devel/act/__pycache__/policy.cpython-38.pyc differ diff --git a/aloha-devel/act/__pycache__/train.cpython-38.pyc b/aloha-devel/act/__pycache__/train.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..2366a6979af6c02855c0547269489efd968448a2 Binary files /dev/null and b/aloha-devel/act/__pycache__/train.cpython-38.pyc differ diff --git a/aloha-devel/act/__pycache__/utils.cpython-310.pyc b/aloha-devel/act/__pycache__/utils.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..ea1052e08882e6f6f700749a9508739346ca59fb Binary files /dev/null and b/aloha-devel/act/__pycache__/utils.cpython-310.pyc differ diff --git a/aloha-devel/act/__pycache__/utils.cpython-38.pyc b/aloha-devel/act/__pycache__/utils.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..5179c5e81da3c0098db817d71ec097e3c84d1e6f Binary files /dev/null and b/aloha-devel/act/__pycache__/utils.cpython-38.pyc differ diff --git a/aloha-devel/act/detr/__pycache__/main.cpython-38.pyc b/aloha-devel/act/detr/__pycache__/main.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..1693f71b5efc6d4da913ed902530261e741da69b Binary files /dev/null and b/aloha-devel/act/detr/__pycache__/main.cpython-38.pyc differ diff --git a/aloha-devel/act/detr/models/__pycache__/__init__.cpython-38.pyc b/aloha-devel/act/detr/models/__pycache__/__init__.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..6ec7fb1573d6975f3dee6dbc523fd4b095b457f5 Binary files /dev/null and b/aloha-devel/act/detr/models/__pycache__/__init__.cpython-38.pyc differ diff --git a/aloha-devel/act/detr/models/__pycache__/backbone.cpython-38.pyc b/aloha-devel/act/detr/models/__pycache__/backbone.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..56ae74886b27a3a77a010875cd8f3b511955286d Binary files /dev/null and b/aloha-devel/act/detr/models/__pycache__/backbone.cpython-38.pyc differ diff --git a/aloha-devel/act/detr/models/__pycache__/detr_vae.cpython-38.pyc b/aloha-devel/act/detr/models/__pycache__/detr_vae.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..5ee9bc98ce8855e1f862d0d5ee5bb1cca652db46 Binary files /dev/null and b/aloha-devel/act/detr/models/__pycache__/detr_vae.cpython-38.pyc differ diff --git a/aloha-devel/act/detr/models/__pycache__/position_encoding.cpython-38.pyc b/aloha-devel/act/detr/models/__pycache__/position_encoding.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..5c15a47cffef2231426f609f7dc27741579e33d0 Binary files /dev/null and b/aloha-devel/act/detr/models/__pycache__/position_encoding.cpython-38.pyc differ diff --git a/aloha-devel/act/detr/models/__pycache__/transformer.cpython-38.pyc b/aloha-devel/act/detr/models/__pycache__/transformer.cpython-38.pyc new file mode 100644 index 0000000000000000000000000000000000000000..ef87592c334bf9d006083a66f01dad8d8caded10 Binary files /dev/null and b/aloha-devel/act/detr/models/__pycache__/transformer.cpython-38.pyc differ diff --git a/aloha-devel/act/detr/models/backbone.py b/aloha-devel/act/detr/models/backbone.py new file mode 100644 index 0000000000000000000000000000000000000000..83920087883bbf03dccfef48852367a94e13c5fa --- /dev/null +++ b/aloha-devel/act/detr/models/backbone.py @@ -0,0 +1,204 @@ +# Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved +""" +Backbone modules. +""" +from collections import OrderedDict + +import torch +import torch.nn.functional as F +import torchvision +from torch import nn +from torchvision.models._utils import IntermediateLayerGetter +from typing import Dict, List + +from ..util.misc import NestedTensor, is_main_process + +from .position_encoding import build_position_encoding + +import IPython +e = IPython.embed + +class FrozenBatchNorm2d(torch.nn.Module): + """ + BatchNorm2d where the batch statistics and the affine parameters are fixed. + + Copy-paste from torchvision.misc.ops with added eps before rqsrt, + without which any other policy_models than torchvision.policy_models.resnet[18,34,50,101] + produce nans. + """ + + def __init__(self, n): + super(FrozenBatchNorm2d, self).__init__() + self.register_buffer("weight", torch.ones(n)) + self.register_buffer("bias", torch.zeros(n)) + self.register_buffer("running_mean", torch.zeros(n)) + self.register_buffer("running_var", torch.ones(n)) + + def _load_from_state_dict(self, state_dict, prefix, local_metadata, strict, + missing_keys, unexpected_keys, error_msgs): + num_batches_tracked_key = prefix + 'num_batches_tracked' + if num_batches_tracked_key in state_dict: + del state_dict[num_batches_tracked_key] + + super(FrozenBatchNorm2d, self)._load_from_state_dict( + state_dict, prefix, local_metadata, strict, + missing_keys, unexpected_keys, error_msgs) + + def forward(self, x): + # move reshapes to the beginning + # to make it fuser-friendly + w = self.weight.reshape(1, -1, 1, 1) + b = self.bias.reshape(1, -1, 1, 1) + rv = self.running_var.reshape(1, -1, 1, 1) + rm = self.running_mean.reshape(1, -1, 1, 1) + eps = 1e-5 + scale = w * (rv + eps).rsqrt() + bias = b - rm * scale + return x * scale + bias + + +class BackboneBase(nn.Module): + + def __init__(self, backbone: nn.Module, train_backbone: bool, num_channels: int, return_interm_layers: bool): + super().__init__() + # for name, parameter in backbone.named_parameters(): # only train later layers # TODO do we want this? + # if not train_backbone or 'layer2' not in name and 'layer3' not in name and 'layer4' not in name: + # parameter.requires_grad_(False) + if return_interm_layers: + return_layers = {"layer1": "0", "layer2": "1", "layer3": "2", "layer4": "3"} + else: + return_layers = {'layer4': "0"} + self.body = IntermediateLayerGetter(backbone, return_layers=return_layers) + self.num_channels = num_channels + + def forward(self, tensor): + xs = self.body(tensor) + return xs + # out: Dict[str, NestedTensor] = {} + # for name, x in xs.items(): + # m = tensor_list.mask + # assert m is not None + # mask = F.interpolate(m[None].float(), size=x.shape[-2:]).to(torch.bool)[0] + # out[name] = NestedTensor(x, mask) + # return out + + +class Backbone(BackboneBase): + """ResNet backbone with frozen BatchNorm.""" + def __init__(self, name: str, + train_backbone: bool, + return_interm_layers: bool, + dilation: bool, + pretrain_backbone_path: str): + backbone = getattr(torchvision.models, name)( + replace_stride_with_dilation=[False, False, dilation], + pretrained=False, norm_layer=FrozenBatchNorm2d) # pretrained # TODO do we want frozen batch_norm?? + + if pretrain_backbone_path: + print(f"loading pretrain backbone from {pretrain_backbone_path}") + checkpoint = torch.load(pretrain_backbone_path, map_location='cpu') + backbone.load_state_dict(checkpoint, strict=False) + + num_channels = 512 if name in ('resnet18', 'resnet34') else 2048 + + super().__init__(backbone, train_backbone, num_channels, return_interm_layers) + + +class Joiner(nn.Sequential): + def __init__(self, backbone, position_embedding): + super().__init__(backbone, position_embedding) + + def forward(self, tensor_list: NestedTensor): + xs = self[0](tensor_list) + out: List[NestedTensor] = [] + pos = [] + for name, x in xs.items(): + out.append(x) + # position encoding + pos.append(self[1](x).to(x.dtype)) + + return out, pos + + +def build_backbone(args): + position_embedding = build_position_encoding(args) + train_backbone = args.lr_backbone > 0 + return_interm_layers = args.masks + pretrain_backbone_path = getattr(args, 'pretrain_backbone_path', '') + backbone = Backbone(args.backbone, train_backbone, return_interm_layers, args.dilation, pretrain_backbone_path) + model = Joiner(backbone, position_embedding) + model.num_channels = backbone.num_channels + return model + + +class RestNetBasicBlock(nn.Module): + def __init__(self, in_channels, out_channels, stride): + super(RestNetBasicBlock, self).__init__() + self.conv1 = nn.Conv2d(in_channels, out_channels, kernel_size=3, stride=stride, padding=1) + self.bn1 = nn.BatchNorm2d(out_channels) + self.conv2 = nn.Conv2d(out_channels, out_channels, kernel_size=3, stride=stride, padding=1) + self.bn2 = nn.BatchNorm2d(out_channels) + + def forward(self, x): + output = self.conv1(x) + output = F.relu(self.bn1(output)) + output = self.conv2(output) + output = self.bn2(output) + return F.relu(x + output) + + +class RestNetDownBlock(nn.Module): + def __init__(self, in_channels, out_channels, stride): + super(RestNetDownBlock, self).__init__() + self.conv1 = nn.Conv2d(in_channels, out_channels, kernel_size=3, stride=stride[0], padding=1) + self.bn1 = nn.BatchNorm2d(out_channels) + self.conv2 = nn.Conv2d(out_channels, out_channels, kernel_size=3, stride=stride[1], padding=1) + self.bn2 = nn.BatchNorm2d(out_channels) + self.extra = nn.Sequential( + nn.Conv2d(in_channels, out_channels, kernel_size=1, stride=stride[0], padding=0), + nn.BatchNorm2d(out_channels) + ) + + def forward(self, x): + extra_x = self.extra(x) + output = self.conv1(x) + out = F.relu(self.bn1(output)) + + out = self.conv2(out) + out = self.bn2(out) + return F.relu(extra_x + out) + + +class DepthNet(nn.Module): + def __init__(self): + super(DepthNet, self).__init__() + self.conv1 = nn.Conv2d(1, 64, kernel_size=7, stride=2, padding=3) + # self.bn1 = nn.BatchNorm2d(64) + # self.maxpool = nn.MaxPool2d(kernel_size=3, stride=2, padding=1) + + # self.layer1 = nn.Sequential(RestNetBasicBlock(64, 64, 1), + # RestNetBasicBlock(64, 64, 1)) + + self.layer2 = nn.Sequential(RestNetDownBlock(64, 128, [4, 1]), + RestNetBasicBlock(128, 128, 1)) + + self.layer3 = nn.Sequential(RestNetDownBlock(128, 256, [4, 1]), + RestNetBasicBlock(256, 256, 1)) + self.num_channels = 256 + # self.layer4 = nn.Sequential(RestNetDownBlock(256, 512, [2, 1]), + # RestNetBasicBlock(512, 512, 1)) + + # self.avgpool = nn.AdaptiveAvgPool2d(output_size=(1, 1)) + # + # self.fc = nn.Linear(512, 10) + + def forward(self, x): + out = self.conv1(x) + # out = self.layer1(out) + out = self.layer2(out) + out = self.layer3(out) + # out = self.layer4(out) + # out = self.avgpool(out) + # out = out.reshape(x.shape[0], -1) + # out = self.fc(out) + return out diff --git a/aloha-devel/act/detr/models/transformer.py b/aloha-devel/act/detr/models/transformer.py new file mode 100644 index 0000000000000000000000000000000000000000..7d8b3633748d9eaae8aba5678d53badc8a6c3187 --- /dev/null +++ b/aloha-devel/act/detr/models/transformer.py @@ -0,0 +1,334 @@ +# Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved +""" +DETR Transformer class. + +Copy-paste from torch.nn.Transformer with modifications: + * positional encodings are passed in MHattention + * extra LN at the end of encoder is removed + * decoder returns a stack of activations from all decoding layers +""" +import copy +from typing import Optional, List + +import torch +import torch.nn.functional as F +from torch import nn, Tensor + +import IPython +e = IPython.embed + +class Transformer(nn.Module): + + def __init__(self, d_model=512, nhead=8, num_encoder_layers=6, + num_decoder_layers=6, dim_feedforward=2048, dropout=0.1, + activation="relu", normalize_before=False, + return_intermediate_dec=False): + super().__init__() + + # 编码层 + encoder_layer = TransformerEncoderLayer(d_model, nhead, dim_feedforward, + dropout, activation, normalize_before) + # 归一化层 + encoder_norm = nn.LayerNorm(d_model) if normalize_before else None + + # 构建多层编码层 + self.encoder = TransformerEncoder(encoder_layer, num_encoder_layers, encoder_norm) + + # 解码层 + decoder_layer = TransformerDecoderLayer(d_model, nhead, dim_feedforward, + dropout, activation, normalize_before) + decoder_norm = nn.LayerNorm(d_model) + + # 构建多层解码层 + self.decoder = TransformerDecoder(decoder_layer, num_decoder_layers, decoder_norm, + return_intermediate=return_intermediate_dec) + + self._reset_parameters() + + self.d_model = d_model + self.nhead = nhead + + def _reset_parameters(self): + for p in self.parameters(): + if p.dim() > 1: + nn.init.xavier_uniform_(p) + + def forward(self, query_embed, + src, pos, is_pad, + robot_state_input, robot_state_pos=None, + latent_input=None, latent_pos=None): + # TODO flatten only when input has H and W + if len(src.shape) == 4: # has H and W + # flatten NxCxHxW to HWxNxC + bs, c, h, w = src.shape + src = src.flatten(2).permute(2, 0, 1) + src_is_pad = torch.full((src.shape[1], src.shape[0]), False).to(src.device) # False: not a padding + latent_is_pad = torch.full((latent_input.shape[1], latent_input.shape[0]), False).to(src.device) # False: not a padding + robot_state_is_pad = torch.full((robot_state_input.shape[1], robot_state_input.shape[0]), False).to(src.device) # False: not a padding + pos = pos.flatten(2).permute(2, 0, 1).repeat(1, bs, 1) + query_embed = query_embed.unsqueeze(1).repeat(1, bs, 1) + # mask = mask.flatten(1) + robot_state_pos = robot_state_pos.unsqueeze(1).repeat(1, bs, 1) # seq, bs, dim + latent_pos = latent_pos.unsqueeze(1).repeat(1, bs, 1) # seq, bs, dim + pos = torch.cat([latent_pos, pos, robot_state_pos], axis=0) + src = torch.cat([latent_input, src, robot_state_input], axis=0) + is_pad = torch.cat([latent_is_pad, src_is_pad, robot_state_is_pad], axis=1) + else: + assert len(src.shape) == 3 + # flatten NxHWxC to HWxNxC + bs, hw, c = src.shape + src = src.permute(1, 0, 2) + pos = pos.unsqueeze(1).repeat(1, bs, 1) + query_embed = query_embed.unsqueeze(1).repeat(1, bs, 1) + + tgt = torch.zeros_like(query_embed) + memory = self.encoder(src, pos=pos, src_key_padding_mask=is_pad) + + hs = self.decoder(tgt, memory, + query_pos=query_embed, + pos=pos, + memory_key_padding_mask=is_pad) + + hs = hs.transpose(1, 2) + return hs + + +class TransformerEncoder(nn.Module): + + def __init__(self, encoder_layer, num_layers, norm=None): + super().__init__() + self.layers = _get_clones(encoder_layer, num_layers) + self.num_layers = num_layers + self.norm = norm + + def forward(self, src, + pos: Optional[Tensor] = None, + src_key_padding_mask: Optional[Tensor] = None, + mask: Optional[Tensor] = None): + output = src + + for layer in self.layers: + output = layer(output, + pos=pos, + src_key_padding_mask=src_key_padding_mask, + src_mask=mask) + + if self.norm is not None: + output = self.norm(output) + + return output + + +class TransformerDecoder(nn.Module): + + def __init__(self, decoder_layer, num_layers, norm=None, return_intermediate=False): + super().__init__() + self.layers = _get_clones(decoder_layer, num_layers) + self.num_layers = num_layers + self.norm = norm + self.return_intermediate = return_intermediate + + def forward(self, tgt, memory, + query_pos: Optional[Tensor] = None, + pos: Optional[Tensor] = None, + tgt_key_padding_mask: Optional[Tensor] = None, + memory_key_padding_mask: Optional[Tensor] = None, + tgt_mask: Optional[Tensor] = None, + memory_mask: Optional[Tensor] = None): + output = tgt + + intermediate = [] + + for layer in self.layers: + output = layer(output, memory, + query_pos=query_pos, + pos=pos, + tgt_key_padding_mask=tgt_key_padding_mask, + memory_key_padding_mask=memory_key_padding_mask, + tgt_mask=tgt_mask, + memory_mask=memory_mask) + if self.return_intermediate: + intermediate.append(self.norm(output)) + + if self.norm is not None: + output = self.norm(output) + if self.return_intermediate: + intermediate.pop() + intermediate.append(output) + + if self.return_intermediate: + return torch.stack(intermediate) + + return output.unsqueeze(0) + + +class TransformerEncoderLayer(nn.Module): + + def __init__(self, d_model, nhead, dim_feedforward=2048, dropout=0.1, + activation="relu", normalize_before=False): + super().__init__() + self.self_attn = nn.MultiheadAttention(d_model, nhead, dropout=dropout) + # Implementation of Feedforward model + self.linear1 = nn.Linear(d_model, dim_feedforward) + self.dropout = nn.Dropout(dropout) + self.linear2 = nn.Linear(dim_feedforward, d_model) + + self.norm1 = nn.LayerNorm(d_model) + self.norm2 = nn.LayerNorm(d_model) + self.dropout1 = nn.Dropout(dropout) + self.dropout2 = nn.Dropout(dropout) + + self.activation = _get_activation_fn(activation) + self.normalize_before = normalize_before + + def with_pos_embed(self, tensor, pos: Optional[Tensor]): + return tensor if pos is None else tensor + pos + + def forward_post(self, + src, + pos: Optional[Tensor] = None, + src_key_padding_mask: Optional[Tensor] = None, + src_mask: Optional[Tensor] = None): + q = k = self.with_pos_embed(src, pos) + src2 = self.self_attn(q, k, value=src, attn_mask=src_mask, + key_padding_mask=src_key_padding_mask)[0] + src = src + self.dropout1(src2) + src = self.norm1(src) + src2 = self.linear2(self.dropout(self.activation(self.linear1(src)))) + src = src + self.dropout2(src2) + src = self.norm2(src) + return src + + def forward_pre(self, src, + pos: Optional[Tensor] = None, + src_key_padding_mask: Optional[Tensor] = None, + src_mask: Optional[Tensor] = None): + src2 = self.norm1(src) + q = k = self.with_pos_embed(src2, pos) + src2 = self.self_attn(q, k, value=src2, attn_mask=src_mask, + key_padding_mask=src_key_padding_mask)[0] + src = src + self.dropout1(src2) + src2 = self.norm2(src) + src2 = self.linear2(self.dropout(self.activation(self.linear1(src2)))) + src = src + self.dropout2(src2) + return src + + def forward(self, src, + pos: Optional[Tensor] = None, + src_key_padding_mask: Optional[Tensor] = None, + src_mask: Optional[Tensor] = None): + if self.normalize_before: + return self.forward_pre(src, pos, src_key_padding_mask, src_mask) + return self.forward_post(src, pos, src_key_padding_mask, src_mask) + + +class TransformerDecoderLayer(nn.Module): + + def __init__(self, d_model, nhead, dim_feedforward=2048, dropout=0.1, + activation="relu", normalize_before=False): + super().__init__() + self.self_attn = nn.MultiheadAttention(d_model, nhead, dropout=dropout) + self.multihead_attn = nn.MultiheadAttention(d_model, nhead, dropout=dropout) + # Implementation of Feedforward model + self.linear1 = nn.Linear(d_model, dim_feedforward) + self.dropout = nn.Dropout(dropout) + self.linear2 = nn.Linear(dim_feedforward, d_model) + + self.norm1 = nn.LayerNorm(d_model) + self.norm2 = nn.LayerNorm(d_model) + self.norm3 = nn.LayerNorm(d_model) + self.dropout1 = nn.Dropout(dropout) + self.dropout2 = nn.Dropout(dropout) + self.dropout3 = nn.Dropout(dropout) + + self.activation = _get_activation_fn(activation) + self.normalize_before = normalize_before + + def with_pos_embed(self, tensor, pos: Optional[Tensor]): + return tensor if pos is None else tensor + pos + + def forward_post(self, tgt, memory, + query_pos: Optional[Tensor] = None, + pos: Optional[Tensor] = None, + tgt_key_padding_mask: Optional[Tensor] = None, + memory_key_padding_mask: Optional[Tensor] = None, + tgt_mask: Optional[Tensor] = None, + memory_mask: Optional[Tensor] = None): + q = k = self.with_pos_embed(tgt, query_pos) + tgt2 = self.self_attn(q, k, value=tgt, attn_mask=tgt_mask, + key_padding_mask=tgt_key_padding_mask)[0] + tgt = tgt + self.dropout1(tgt2) + tgt = self.norm1(tgt) + tgt2 = self.multihead_attn(query=self.with_pos_embed(tgt, query_pos), + key=self.with_pos_embed(memory, pos), + value=memory, attn_mask=memory_mask, + key_padding_mask=memory_key_padding_mask)[0] + tgt = tgt + self.dropout2(tgt2) + tgt = self.norm2(tgt) + tgt2 = self.linear2(self.dropout(self.activation(self.linear1(tgt)))) + tgt = tgt + self.dropout3(tgt2) + tgt = self.norm3(tgt) + return tgt + + def forward_pre(self, tgt, memory, + query_pos: Optional[Tensor] = None, + pos: Optional[Tensor] = None, + tgt_key_padding_mask: Optional[Tensor] = None, + memory_key_padding_mask: Optional[Tensor] = None, + tgt_mask: Optional[Tensor] = None, + memory_mask: Optional[Tensor] = None): + tgt2 = self.norm1(tgt) + q = k = self.with_pos_embed(tgt2, query_pos) + tgt2 = self.self_attn(q, k, value=tgt2, attn_mask=tgt_mask, + key_padding_mask=tgt_key_padding_mask)[0] + tgt = tgt + self.dropout1(tgt2) + tgt2 = self.norm2(tgt) + tgt2 = self.multihead_attn(query=self.with_pos_embed(tgt2, query_pos), + key=self.with_pos_embed(memory, pos), + value=memory, attn_mask=memory_mask, + key_padding_mask=memory_key_padding_mask)[0] + tgt = tgt + self.dropout2(tgt2) + tgt2 = self.norm3(tgt) + tgt2 = self.linear2(self.dropout(self.activation(self.linear1(tgt2)))) + tgt = tgt + self.dropout3(tgt2) + return tgt + + def forward(self, tgt, memory, + query_pos: Optional[Tensor] = None, + pos: Optional[Tensor] = None, + tgt_key_padding_mask: Optional[Tensor] = None, + memory_key_padding_mask: Optional[Tensor] = None, + tgt_mask: Optional[Tensor] = None, + memory_mask: Optional[Tensor] = None): + if self.normalize_before: + return self.forward_pre(tgt, memory, query_pos, pos, + tgt_key_padding_mask, memory_key_padding_mask, tgt_mask, memory_mask) + return self.forward_post(tgt, memory, query_pos, pos, + tgt_key_padding_mask, memory_key_padding_mask, tgt_mask, memory_mask) + + +def _get_clones(module, N): + return nn.ModuleList([copy.deepcopy(module) for i in range(N)]) + + +def build_transformer(args): + return Transformer( + d_model=args.hidden_dim, + dropout=args.dropout, + nhead=args.nheads, + dim_feedforward=args.dim_feedforward, + num_encoder_layers=args.enc_layers, + num_decoder_layers=args.dec_layers, + normalize_before=args.pre_norm, + return_intermediate_dec=True) + + +def _get_activation_fn(activation): + """Return an activation function given a string""" + if activation == "relu": + return F.relu + if activation == "gelu": + return F.gelu + if activation == "glu": + return F.glu + raise RuntimeError(F"activation should be relu/gelu, not {activation}.") diff --git a/aloha-devel/act/detr/util/__init__.py b/aloha-devel/act/detr/util/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..168f9979a4623806934b0ff1102ac166704e7dec --- /dev/null +++ b/aloha-devel/act/detr/util/__init__.py @@ -0,0 +1 @@ +# Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved diff --git a/aloha-devel/act/detr/util/box_ops.py b/aloha-devel/act/detr/util/box_ops.py new file mode 100644 index 0000000000000000000000000000000000000000..9c088e5bacc88ff7217fc971f5db889f5bb45b39 --- /dev/null +++ b/aloha-devel/act/detr/util/box_ops.py @@ -0,0 +1,88 @@ +# Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved +""" +Utilities for bounding box manipulation and GIoU. +""" +import torch +from torchvision.ops.boxes import box_area + + +def box_cxcywh_to_xyxy(x): + x_c, y_c, w, h = x.unbind(-1) + b = [(x_c - 0.5 * w), (y_c - 0.5 * h), + (x_c + 0.5 * w), (y_c + 0.5 * h)] + return torch.stack(b, dim=-1) + + +def box_xyxy_to_cxcywh(x): + x0, y0, x1, y1 = x.unbind(-1) + b = [(x0 + x1) / 2, (y0 + y1) / 2, + (x1 - x0), (y1 - y0)] + return torch.stack(b, dim=-1) + + +# modified from torchvision to also return the union +def box_iou(boxes1, boxes2): + area1 = box_area(boxes1) + area2 = box_area(boxes2) + + lt = torch.max(boxes1[:, None, :2], boxes2[:, :2]) # [N,M,2] + rb = torch.min(boxes1[:, None, 2:], boxes2[:, 2:]) # [N,M,2] + + wh = (rb - lt).clamp(min=0) # [N,M,2] + inter = wh[:, :, 0] * wh[:, :, 1] # [N,M] + + union = area1[:, None] + area2 - inter + + iou = inter / union + return iou, union + + +def generalized_box_iou(boxes1, boxes2): + """ + Generalized IoU from https://giou.stanford.edu/ + + The boxes should be in [x0, y0, x1, y1] format + + Returns a [N, M] pairwise matrix, where N = len(boxes1) + and M = len(boxes2) + """ + # degenerate boxes gives inf / nan results + # so do an early check + assert (boxes1[:, 2:] >= boxes1[:, :2]).all() + assert (boxes2[:, 2:] >= boxes2[:, :2]).all() + iou, union = box_iou(boxes1, boxes2) + + lt = torch.min(boxes1[:, None, :2], boxes2[:, :2]) + rb = torch.max(boxes1[:, None, 2:], boxes2[:, 2:]) + + wh = (rb - lt).clamp(min=0) # [N,M,2] + area = wh[:, :, 0] * wh[:, :, 1] + + return iou - (area - union) / area + + +def masks_to_boxes(masks): + """Compute the bounding boxes around the provided masks + + The masks should be in format [N, H, W] where N is the number of masks, (H, W) are the spatial dimensions. + + Returns a [N, 4] tensors, with the boxes in xyxy format + """ + if masks.numel() == 0: + return torch.zeros((0, 4), device=masks.device) + + h, w = masks.shape[-2:] + + y = torch.arange(0, h, dtype=torch.float) + x = torch.arange(0, w, dtype=torch.float) + y, x = torch.meshgrid(y, x) + + x_mask = (masks * x.unsqueeze(0)) + x_max = x_mask.flatten(1).max(-1)[0] + x_min = x_mask.masked_fill(~(masks.bool()), 1e8).flatten(1).min(-1)[0] + + y_mask = (masks * y.unsqueeze(0)) + y_max = y_mask.flatten(1).max(-1)[0] + y_min = y_mask.masked_fill(~(masks.bool()), 1e8).flatten(1).min(-1)[0] + + return torch.stack([x_min, y_min, x_max, y_max], 1) diff --git a/aloha-devel/act/detr/util/plot_utils.py b/aloha-devel/act/detr/util/plot_utils.py new file mode 100644 index 0000000000000000000000000000000000000000..0f24bed0d3fe4624aeb231ddd02633f2e58e4bff --- /dev/null +++ b/aloha-devel/act/detr/util/plot_utils.py @@ -0,0 +1,107 @@ +""" +Plotting utilities to visualize training logs. +""" +import torch +import pandas as pd +import numpy as np +import seaborn as sns +import matplotlib.pyplot as plt + +from pathlib import Path, PurePath + + +def plot_logs(logs, fields=('class_error', 'loss_bbox_unscaled', 'mAP'), ewm_col=0, log_name='log.txt'): + ''' + Function to plot specific fields from training log(s). Plots both training and test results. + + :: Inputs - logs = list containing Path objects, each pointing to individual dir with a log file + - fields = which results to plot from each log file - plots both training and test for each field. + - ewm_col = optional, which column to use as the exponential weighted smoothing of the plots + - log_name = optional, name of log file if different than default 'log.txt'. + + :: Outputs - matplotlib plots of results in fields, color coded for each log file. + - solid lines are training results, dashed lines are test results. + + ''' + func_name = "plot_utils.py::plot_logs" + + # verify logs is a list of Paths (list[Paths]) or single Pathlib object Path, + # convert single Path to list to avoid 'not iterable' error + + if not isinstance(logs, list): + if isinstance(logs, PurePath): + logs = [logs] + print(f"{func_name} info: logs param expects a list argument, converted to list[Path].") + else: + raise ValueError(f"{func_name} - invalid argument for logs parameter.\n \ + Expect list[Path] or single Path obj, received {type(logs)}") + + # Quality checks - verify valid dir(s), that every item in list is Path object, and that log_name exists in each dir + for i, dir in enumerate(logs): + if not isinstance(dir, PurePath): + raise ValueError(f"{func_name} - non-Path object in logs argument of {type(dir)}: \n{dir}") + if not dir.exists(): + raise ValueError(f"{func_name} - invalid directory in logs argument:\n{dir}") + # verify log_name exists + fn = Path(dir / log_name) + if not fn.exists(): + print(f"-> missing {log_name}. Have you gotten to Epoch 1 in training?") + print(f"--> full path of missing log file: {fn}") + return + + # load log file(s) and plot + dfs = [pd.read_json(Path(p) / log_name, lines=True) for p in logs] + + fig, axs = plt.subplots(ncols=len(fields), figsize=(16, 5)) + + for df, color in zip(dfs, sns.color_palette(n_colors=len(logs))): + for j, field in enumerate(fields): + if field == 'mAP': + coco_eval = pd.DataFrame( + np.stack(df.test_coco_eval_bbox.dropna().values)[:, 1] + ).ewm(com=ewm_col).mean() + axs[j].plot(coco_eval, c=color) + else: + df.interpolate().ewm(com=ewm_col).mean().plot( + y=[f'train_{field}', f'test_{field}'], + ax=axs[j], + color=[color] * 2, + style=['-', '--'] + ) + for ax, field in zip(axs, fields): + ax.legend([Path(p).name for p in logs]) + ax.set_title(field) + + +def plot_precision_recall(files, naming_scheme='iter'): + if naming_scheme == 'exp_id': + # name becomes exp_id + names = [f.parts[-3] for f in files] + elif naming_scheme == 'iter': + names = [f.stem for f in files] + else: + raise ValueError(f'not supported {naming_scheme}') + fig, axs = plt.subplots(ncols=2, figsize=(16, 5)) + for f, color, name in zip(files, sns.color_palette("Blues", n_colors=len(files)), names): + data = torch.load(f) + # precision is n_iou, n_points, n_cat, n_area, max_det + precision = data['precision'] + recall = data['params'].recThrs + scores = data['scores'] + # take precision for all classes, all areas and 100 detections + precision = precision[0, :, :, 0, -1].mean(1) + scores = scores[0, :, :, 0, -1].mean(1) + prec = precision.mean() + rec = data['recall'][0, :, 0, -1].mean() + print(f'{naming_scheme} {name}: mAP@50={prec * 100: 05.1f}, ' + + f'score={scores.mean():0.3f}, ' + + f'f1={2 * prec * rec / (prec + rec + 1e-8):0.3f}' + ) + axs[0].plot(recall, precision, c=color) + axs[1].plot(recall, scores, c=color) + + axs[0].set_title('Precision / Recall') + axs[0].legend(names) + axs[1].set_title('Scores / Recall') + axs[1].legend(names) + return fig, axs diff --git a/aloha-devel/act/scripts/test.sh b/aloha-devel/act/scripts/test.sh new file mode 100644 index 0000000000000000000000000000000000000000..e6e34589537c0a0f767ae57375bf629403273b6a --- /dev/null +++ b/aloha-devel/act/scripts/test.sh @@ -0,0 +1,51 @@ +#!/bin/bash + +# 来自您训练脚本的参数 +HIDDEN_DIM=1024 +DIM_FEEDFORWARD=4096 +DROPOUT=0.0 +TASK_NAME="blue_new" + +ROOT="/inspire/hdd/ws-f4d69b29-e0a5-44e6-bd92-acf4de9990f0/public-project/chengdongzhou-240108390137/vla_projects/cobot_magic" +TRAIN_DIR="$ROOT/tranin_dir" +PRETRAIN_BACKBONE_PATH="/inspire/hdd/ws-f4d69b29-e0a5-44e6-bd92-acf4de9990f0/public-project/chengdongzhou-240108390137/ai_models/resnets/resnet18-f37072fd.pth" + +# --- SCRIPT LOGIC --- +CKPT_DIR="$TRAIN_DIR/$TASK_NAME" +CKPT_PATH="$CKPT_DIR/policy_epoch_500_seed_0.ckpt" + +if [ ! -d "act" ]; then + echo "错误：此脚本必须在项目的根目录下运行。" + exit 1 +fi + +if [ ! -f "$CKPT_PATH" ]; then + echo "错误：在以下路径未找到检查点文件: $CKPT_PATH" + echo "请检查以下内容:" + echo "1. 'ROOT', 'TRAIN_DIR', 和 'TASK_NAME' 变量是否正确。" + echo "2. 您是否已在此脚本顶部正确设置了 'NO_PRETRAIN' 变量。" + exit 1 +fi + +echo "======================================================" +echo "开始推理内存测试" +echo "======================================================" +echo "策略类别: ACT (default)" +echo "检查点: $CKPT_PATH" +echo "任务名称: $TASK_NAME" +echo "隐藏层维度: $HIDDEN_DIM" +echo "前馈网络维度: $DIM_FEEDFORWARD" +echo "======================================================" + +python act/test_inference.py \ + --policy_class ACT \ + --ckpt_path "$CKPT_PATH" \ + --task_name "$TASK_NAME" \ + --hidden_dim $HIDDEN_DIM \ + --dim_feedforward $DIM_FEEDFORWARD \ + --dropout $DROPOUT \ + --pretrain_backbone_path "$PRETRAIN_BACKBONE_PATH" + +echo "======================================================" +echo "测试完成。" +echo "======================================================" \ No newline at end of file diff --git a/aloha-devel/act/scripts/train_blue.sh b/aloha-devel/act/scripts/train_blue.sh new file mode 100644 index 0000000000000000000000000000000000000000..e23fd426b02c06af48dac699a36b85693d5d4d5b --- /dev/null +++ b/aloha-devel/act/scripts/train_blue.sh @@ -0,0 +1,43 @@ +num_epochs=5000 +batch_size=16 +num_episodes=80 +ckpt_save_interval=500 +lr=4e-5 +dropout=0.0 +lr_backbone=4e-5 +plot_interval=100 +hidden_dim=1024 +dim_feedforward=4096 +lr_decay_start_epoch=3000 +ROOT=/inspire/hdd/ws-f4d69b29-e0a5-44e6-bd92-acf4de9990f0/public-project/chengdongzhou-240108390137/vla_projects/cobot_magic +train_dir=$ROOT/tranin_dir +pretrain_ckpt=$ROOT/policy_best.ckpt +ws_path=$(pwd) +dataset_dir=/inspire/ssd/ws-f4d69b29-e0a5-44e6-bd92-acf4de9990f0/public-project/public/aloha_group +pretrain_backbone_path=/inspire/hdd/ws-f4d69b29-e0a5-44e6-bd92-acf4de9990f0/public-project/chengdongzhou-240108390137/ai_models/resnets/resnet18-f37072fd.pth +# echo "$pretrain_ckpt" +# echo "$train_dir" +# echo $(pwd) + + +cd $ws_path +task_name=blue_new +python act/train.py \ + --dataset $dataset_dir \ + --ckpt_dir $train_dir/$task_name/$no_pretrain \ + --batch_size $batch_size \ + --num_epochs $num_epochs \ + --num_episodes $num_episodes \ + --task_name $task_name \ + --pretrain_backbone_path $pretrain_backbone_path \ + --ckpt_save_interval $ckpt_save_interval \ + --plot_interval $plot_interval \ + --lr $lr \ + --dropout $dropout\ + --lr_backbone $lr_backbone \ + --hidden_dim $hidden_dim \ + --dim_feedforward $dim_feedforward \ + --lr_decay_start_epoch $lr_decay_start_epoch + + +# python -m debugpy --listen 1234 --wait-for-client act/train.py --dataset $dataset_dir --ckpt_dir $train_dir/no_pretrain --batch_size $batch_size --num_epochs $num_epochs --num_episodes $num_episodes --task_name $task_name \ No newline at end of file diff --git a/aloha-devel/act/scripts/train_purple.sh b/aloha-devel/act/scripts/train_purple.sh new file mode 100644 index 0000000000000000000000000000000000000000..a054b34657c5d9e6b04913fc68f0f0a6a9324256 --- /dev/null +++ b/aloha-devel/act/scripts/train_purple.sh @@ -0,0 +1,44 @@ +num_epochs=5000 +batch_size=16 +num_episodes=64 +ckpt_save_interval=500 +lr=4e-5 +dropout=0.0 +lr_backbone=4e-5 +plot_interval=100 +hidden_dim=1024 +dim_feedforward=4096 +lr_decay_start_epoch=3000 +ROOT=/inspire/hdd/ws-f4d69b29-e0a5-44e6-bd92-acf4de9990f0/public-project/chengdongzhou-240108390137/vla_projects/cobot_magic +train_dir=$ROOT/tranin_dir +pretrain_ckpt=$ROOT/policy_best.ckpt +ws_path=$(pwd) +dataset_dir=/inspire/ssd/ws-f4d69b29-e0a5-44e6-bd92-acf4de9990f0/public-project/public/aloha_group +pretrain_backbone_path=/inspire/hdd/ws-f4d69b29-e0a5-44e6-bd92-acf4de9990f0/public-project/chengdongzhou-240108390137/ai_models/resnets/resnet18-f37072fd.pth +# echo "$pretrain_ckpt" +# echo "$train_dir" +# echo $(pwd) + + +cd $ws_path +task_name=purple +python act/train.py \ + --dataset $dataset_dir \ + --ckpt_dir $train_dir/$task_name/$no_pretrain \ + --batch_size $batch_size \ + --num_epochs $num_epochs \ + --num_episodes $num_episodes \ + --task_name $task_name \ + --pretrain_backbone_path $pretrain_backbone_path \ + --ckpt_save_interval $ckpt_save_interval \ + --plot_interval $plot_interval \ + --lr $lr \ + --dropout $dropout\ + --lr_backbone $lr_backbone \ + --hidden_dim $hidden_dim \ + --dim_feedforward $dim_feedforward \ + --lr_decay_start_epoch $lr_decay_start_epoch + + + +# python -m debugpy --listen 1234 --wait-for-client act/train.py --dataset $dataset_dir --ckpt_dir $train_dir/no_pretrain --batch_size $batch_size --num_epochs $num_epochs --num_episodes $num_episodes --task_name $task_name \ No newline at end of file diff --git a/camera_ws/build/CMakeFiles/3.16.3/CMakeCXXCompiler.cmake b/camera_ws/build/CMakeFiles/3.16.3/CMakeCXXCompiler.cmake new file mode 100644 index 0000000000000000000000000000000000000000..278ef39ee396e9c0d852a7fc8f2647f7da42a20b --- /dev/null +++ b/camera_ws/build/CMakeFiles/3.16.3/CMakeCXXCompiler.cmake @@ -0,0 +1,88 @@ +set(CMAKE_CXX_COMPILER "/usr/bin/c++") +set(CMAKE_CXX_COMPILER_ARG1 "") +set(CMAKE_CXX_COMPILER_ID "GNU") +set(CMAKE_CXX_COMPILER_VERSION "9.4.0") +set(CMAKE_CXX_COMPILER_VERSION_INTERNAL "") +set(CMAKE_CXX_COMPILER_WRAPPER "") +set(CMAKE_CXX_STANDARD_COMPUTED_DEFAULT "14") +set(CMAKE_CXX_COMPILE_FEATURES "cxx_std_98;cxx_template_template_parameters;cxx_std_11;cxx_alias_templates;cxx_alignas;cxx_alignof;cxx_attributes;cxx_auto_type;cxx_constexpr;cxx_decltype;cxx_decltype_incomplete_return_types;cxx_default_function_template_args;cxx_defaulted_functions;cxx_defaulted_move_initializers;cxx_delegating_constructors;cxx_deleted_functions;cxx_enum_forward_declarations;cxx_explicit_conversions;cxx_extended_friend_declarations;cxx_extern_templates;cxx_final;cxx_func_identifier;cxx_generalized_initializers;cxx_inheriting_constructors;cxx_inline_namespaces;cxx_lambdas;cxx_local_type_template_args;cxx_long_long_type;cxx_noexcept;cxx_nonstatic_member_init;cxx_nullptr;cxx_override;cxx_range_for;cxx_raw_string_literals;cxx_reference_qualified_functions;cxx_right_angle_brackets;cxx_rvalue_references;cxx_sizeof_member;cxx_static_assert;cxx_strong_enums;cxx_thread_local;cxx_trailing_return_types;cxx_unicode_literals;cxx_uniform_initialization;cxx_unrestricted_unions;cxx_user_literals;cxx_variadic_macros;cxx_variadic_templates;cxx_std_14;cxx_aggregate_default_initializers;cxx_attribute_deprecated;cxx_binary_literals;cxx_contextual_conversions;cxx_decltype_auto;cxx_digit_separators;cxx_generic_lambdas;cxx_lambda_init_captures;cxx_relaxed_constexpr;cxx_return_type_deduction;cxx_variable_templates;cxx_std_17;cxx_std_20") +set(CMAKE_CXX98_COMPILE_FEATURES "cxx_std_98;cxx_template_template_parameters") +set(CMAKE_CXX11_COMPILE_FEATURES "cxx_std_11;cxx_alias_templates;cxx_alignas;cxx_alignof;cxx_attributes;cxx_auto_type;cxx_constexpr;cxx_decltype;cxx_decltype_incomplete_return_types;cxx_default_function_template_args;cxx_defaulted_functions;cxx_defaulted_move_initializers;cxx_delegating_constructors;cxx_deleted_functions;cxx_enum_forward_declarations;cxx_explicit_conversions;cxx_extended_friend_declarations;cxx_extern_templates;cxx_final;cxx_func_identifier;cxx_generalized_initializers;cxx_inheriting_constructors;cxx_inline_namespaces;cxx_lambdas;cxx_local_type_template_args;cxx_long_long_type;cxx_noexcept;cxx_nonstatic_member_init;cxx_nullptr;cxx_override;cxx_range_for;cxx_raw_string_literals;cxx_reference_qualified_functions;cxx_right_angle_brackets;cxx_rvalue_references;cxx_sizeof_member;cxx_static_assert;cxx_strong_enums;cxx_thread_local;cxx_trailing_return_types;cxx_unicode_literals;cxx_uniform_initialization;cxx_unrestricted_unions;cxx_user_literals;cxx_variadic_macros;cxx_variadic_templates") +set(CMAKE_CXX14_COMPILE_FEATURES "cxx_std_14;cxx_aggregate_default_initializers;cxx_attribute_deprecated;cxx_binary_literals;cxx_contextual_conversions;cxx_decltype_auto;cxx_digit_separators;cxx_generic_lambdas;cxx_lambda_init_captures;cxx_relaxed_constexpr;cxx_return_type_deduction;cxx_variable_templates") +set(CMAKE_CXX17_COMPILE_FEATURES "cxx_std_17") +set(CMAKE_CXX20_COMPILE_FEATURES "cxx_std_20") + +set(CMAKE_CXX_PLATFORM_ID "Linux") +set(CMAKE_CXX_SIMULATE_ID "") +set(CMAKE_CXX_COMPILER_FRONTEND_VARIANT "") +set(CMAKE_CXX_SIMULATE_VERSION "") + + + +set(CMAKE_AR "/usr/bin/ar") +set(CMAKE_CXX_COMPILER_AR "/usr/bin/gcc-ar-9") +set(CMAKE_RANLIB "/usr/bin/ranlib") +set(CMAKE_CXX_COMPILER_RANLIB "/usr/bin/gcc-ranlib-9") +set(CMAKE_LINKER "/usr/bin/ld") +set(CMAKE_MT "") +set(CMAKE_COMPILER_IS_GNUCXX 1) +set(CMAKE_CXX_COMPILER_LOADED 1) +set(CMAKE_CXX_COMPILER_WORKS TRUE) +set(CMAKE_CXX_ABI_COMPILED TRUE) +set(CMAKE_COMPILER_IS_MINGW ) +set(CMAKE_COMPILER_IS_CYGWIN ) +if(CMAKE_COMPILER_IS_CYGWIN) + set(CYGWIN 1) + set(UNIX 1) +endif() + +set(CMAKE_CXX_COMPILER_ENV_VAR "CXX") + +if(CMAKE_COMPILER_IS_MINGW) + set(MINGW 1) +endif() +set(CMAKE_CXX_COMPILER_ID_RUN 1) +set(CMAKE_CXX_SOURCE_FILE_EXTENSIONS C;M;c++;cc;cpp;cxx;m;mm;CPP) +set(CMAKE_CXX_IGNORE_EXTENSIONS inl;h;hpp;HPP;H;o;O;obj;OBJ;def;DEF;rc;RC) + +foreach (lang C OBJC OBJCXX) + if (CMAKE_${lang}_COMPILER_ID_RUN) + foreach(extension IN LISTS CMAKE_${lang}_SOURCE_FILE_EXTENSIONS) + list(REMOVE_ITEM CMAKE_CXX_SOURCE_FILE_EXTENSIONS ${extension}) + endforeach() + endif() +endforeach() + +set(CMAKE_CXX_LINKER_PREFERENCE 30) +set(CMAKE_CXX_LINKER_PREFERENCE_PROPAGATES 1) + +# Save compiler ABI information. +set(CMAKE_CXX_SIZEOF_DATA_PTR "8") +set(CMAKE_CXX_COMPILER_ABI "ELF") +set(CMAKE_CXX_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") + +if(CMAKE_CXX_SIZEOF_DATA_PTR) + set(CMAKE_SIZEOF_VOID_P "${CMAKE_CXX_SIZEOF_DATA_PTR}") +endif() + +if(CMAKE_CXX_COMPILER_ABI) + set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_CXX_COMPILER_ABI}") +endif() + +if(CMAKE_CXX_LIBRARY_ARCHITECTURE) + set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") +endif() + +set(CMAKE_CXX_CL_SHOWINCLUDES_PREFIX "") +if(CMAKE_CXX_CL_SHOWINCLUDES_PREFIX) + set(CMAKE_CL_SHOWINCLUDES_PREFIX "${CMAKE_CXX_CL_SHOWINCLUDES_PREFIX}") +endif() + + + + + +set(CMAKE_CXX_IMPLICIT_INCLUDE_DIRECTORIES "/usr/include/c++/9;/usr/include/x86_64-linux-gnu/c++/9;/usr/include/c++/9/backward;/usr/lib/gcc/x86_64-linux-gnu/9/include;/usr/local/include;/usr/include/x86_64-linux-gnu;/usr/include") +set(CMAKE_CXX_IMPLICIT_LINK_LIBRARIES "stdc++;m;gcc_s;gcc;c;gcc_s;gcc") +set(CMAKE_CXX_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/9;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") +set(CMAKE_CXX_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") diff --git a/camera_ws/src/realsense-ros/NOTICE b/camera_ws/src/realsense-ros/NOTICE new file mode 100644 index 0000000000000000000000000000000000000000..f98628701293a092dc24ea2b46c47b94eb76b0d5 --- /dev/null +++ b/camera_ws/src/realsense-ros/NOTICE @@ -0,0 +1,4 @@ +This project uses code from the following third-party projects, listed here +with the full text of their respective licenses. + +ddynamic_reconfigure (BSD) - https://github.com/awesomebytes/ddynamic_reconfigure diff --git a/camera_ws/src/realsense-ros/README.md b/camera_ws/src/realsense-ros/README.md new file mode 100644 index 0000000000000000000000000000000000000000..70732d0a65bc27fc850cd1ac1fc4e9f14e7ed1ca --- /dev/null +++ b/camera_ws/src/realsense-ros/README.md @@ -0,0 +1,335 @@ +# ROS Wrapper for Intel® RealSense™ Devices +These are packages for using Intel RealSense cameras (D400 series SR300 camera and T265 Tracking Module) with ROS. + +This version supports Kinetic, Melodic and Noetic distributions. + +For running in ROS2 environment please switch to the [ros2 branch](https://github.com/IntelRealSense/realsense-ros/tree/ros2).
+ +LibRealSense2 supported version: v2.50.0 (see [realsense2_camera release notes](https://github.com/IntelRealSense/realsense-ros/releases)) + +## Installation Instructions + +### Ubuntu + #### Step 1: Install the ROS distribution + - #### Install [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu), on Ubuntu 16.04, [ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) on Ubuntu 18.04 or [ROS Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) on Ubuntu 20.04. + +### Windows + #### Step 1: Install the ROS distribution + - #### Install [ROS Melodic or later on Windows 10](https://wiki.ros.org/Installation/Windows) + + +### There are 2 sources to install realsense2_camera from: + +* ### Method 1: The ROS distribution: + + *Ubuntu* + + realsense2_camera is available as a debian package of ROS distribution. It can be installed by typing: + + ```sudo apt-get install ros-$ROS_DISTRO-realsense2-camera``` + + This will install both realsense2_camera and its dependents, including librealsense2 library and matching udev-rules. + + Notice: + * The version of librealsense2 is almost always behind the one availeable in RealSense™ official repository. + * librealsense2 is not built to use native v4l2 driver but the less stable RS-USB protocol. That is because the last is more general and operational on a larger variety of platforms. + * realsense2_description is available as a separate debian package of ROS distribution. It includes the 3D-models of the devices and is necessary for running launch files that include these models (i.e. rs_d435_camera_with_model.launch). It can be installed by typing: + `sudo apt-get install ros-$ROS_DISTRO-realsense2-description` + + *Windows* + + **Chocolatey distribution Coming soon** + +* ### Method 2: The RealSense™ distribution: + > This option is demonstrated in the [.travis.yml](https://github.com/intel-ros/realsense/blob/development/.travis.yml) file. It basically summerize the elaborate instructions in the following 2 steps: + + + ### Step 1: Install the latest Intel® RealSense™ SDK 2.0 + + *Ubuntu* + + Install librealsense2 debian package: + * Jetson users - use the [Jetson Installation Guide](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation_jetson.md) + * Otherwise, install from [Linux Debian Installation Guide](https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md#installing-the-packages) + - In that case treat yourself as a developer. Make sure you follow the instructions to also install librealsense2-dev and librealsense2-dkms packages. + + *Windows* + Install using vcpkg + + `vcpkg install realsense2:x64-windows` + + #### OR + - #### Build from sources by downloading the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.50.0) and follow the instructions under [Linux Installation](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md) + + + ### Step 2: Install Intel® RealSense™ ROS from Sources + - Create a [catkin](http://wiki.ros.org/catkin#Installing_catkin) workspace + *Ubuntu* + ```bash + mkdir -p ~/catkin_ws/src + cd ~/catkin_ws/src/ + ``` + *Windows* + ```batch + mkdir c:\catkin_ws\src + cd c:\catkin_ws\src + ``` + + - Clone the latest Intel® RealSense™ ROS from [here](https://github.com/intel-ros/realsense/releases) into 'catkin_ws/src/' + ```bashrc + git clone https://github.com/IntelRealSense/realsense-ros.git + cd realsense-ros/ + git checkout `git tag | sort -V | grep -P "^2.\d+\.\d+" | tail -1` + cd .. + ``` + - Make sure all dependent packages are installed. You can check .travis.yml file for reference. + - Specifically, make sure that the ros package *ddynamic_reconfigure* is installed. If *ddynamic_reconfigure* cannot be installed using APT or if you are using *Windows* you may clone it into your workspace 'catkin_ws/src/' from [here](https://github.com/pal-robotics/ddynamic_reconfigure/tree/kinetic-devel) + + + ```bash + catkin_init_workspace + cd .. + catkin_make clean + catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release + catkin_make install + ``` + + *Ubuntu* + ```bash + echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc + source ~/.bashrc + ``` + + *Windows* + ```batch + devel\setup.bat + ``` + +## Usage Instructions + +### Start the camera node +To start the camera node in ROS: + +```bash +roslaunch realsense2_camera rs_camera.launch +``` + +This will stream all camera sensors and publish on the appropriate ROS topics. + +Other stream resolutions and frame rates can optionally be provided as parameters to the 'rs_camera.launch' file. + +### Published Topics +The published topics differ according to the device and parameters. +After running the above command with D435i attached, the following list of topics will be available (This is a partial list. For full one type `rostopic list`): +- /camera/color/camera_info +- /camera/color/image_raw +- /camera/color/metadata +- /camera/depth/camera_info +- /camera/depth/image_rect_raw +- /camera/depth/metadata +- /camera/extrinsics/depth_to_color +- /camera/extrinsics/depth_to_infra1 +- /camera/extrinsics/depth_to_infra2 +- /camera/infra1/camera_info +- /camera/infra1/image_rect_raw +- /camera/infra2/camera_info +- /camera/infra2/image_rect_raw +- /camera/gyro/imu_info +- /camera/gyro/metadata +- /camera/gyro/sample +- /camera/accel/imu_info +- /camera/accel/metadata +- /camera/accel/sample +- /diagnostics + +>Using an L515 device the list differs a little by adding a 4-bit confidence grade (pulished as a mono8 image): +>- /camera/confidence/camera_info +>- /camera/confidence/image_rect_raw +> +>It also replaces the 2 infrared topics with the single available one: +>- /camera/infra/camera_info +>- /camera/infra/image_raw + + +The "/camera" prefix is the default and can be changed. Check the rs_multiple_devices.launch file for an example. +If using D435 or D415, the gyro and accel topics wont be available. Likewise, other topics will be available when using T265 (see below). + +### Launch parameters +The following parameters are available by the wrapper: +- **serial_no**: will attach to the device with the given serial number (*serial_no*) number. Default, attach to available RealSense device in random. +- **usb_port_id**: will attach to the device with the given USB port (*usb_port_id*). i.e 4-1, 4-2 etc. Default, ignore USB port when choosing a device. +- **device_type**: will attach to a device whose name includes the given *device_type* regular expression pattern. Default, ignore device type. For example, device_type:=d435 will match d435 and d435i. device_type=d435(?!i) will match d435 but not d435i. + +- **rosbag_filename**: Will publish topics from rosbag file. +- **initial_reset**: On occasions the device was not closed properly and due to firmware issues needs to reset. If set to true, the device will reset prior to usage. +- **reconnect_timeout**: When the driver cannot connect to the device try to reconnect after this timeout (in seconds). +- **align_depth**: If set to true, will publish additional topics for the "aligned depth to color" image.: ```/camera/aligned_depth_to_color/image_raw```, ```/camera/aligned_depth_to_color/camera_info```.
+The pointcloud, if enabled, will be built based on the aligned_depth_to_color image.
+- **filters**: any of the following options, separated by commas:
+ - ```colorizer```: will color the depth image. On the depth topic an RGB image will be published, instead of the 16bit depth values . + - ```pointcloud```: will add a pointcloud topic `/camera/depth/color/points`. + * The texture of the pointcloud can be modified in rqt_reconfigure (see below) or using the parameters: `pointcloud_texture_stream` and `pointcloud_texture_index`. Run rqt_reconfigure to see available values for these parameters.
+ * The depth FOV and the texture FOV are not similar. By default, pointcloud is limited to the section of depth containing the texture. You can have a full depth to pointcloud, coloring the regions beyond the texture with zeros, by setting `allow_no_texture_points` to true. + * pointcloud is of an unordered format by default. This can be changed by setting `ordered_pc` to true. +- ```hdr_merge```: Allows depth image to be created by merging the information from 2 consecutive frames, taken with different exposure and gain values. The way to set exposure and gain values for each sequence in runtime is by first selecting the sequence id, using rqt_reconfigure `stereo_module/sequence_id` parameter and then modifying the `stereo_module/gain`, and `stereo_module/exposure`.
To view the effect on the infrared image for each sequence id use the `sequence_id_filter/sequence_id` parameter.
To initialize these parameters in start time use the following parameters:
+ `stereo_module/exposure/1`, `stereo_module/gain/1`, `stereo_module/exposure/2`, `stereo_module/gain/2`
+ \* For in-depth review of the subject please read the accompanying [white paper](https://dev.intelrealsense.com/docs/high-dynamic-range-with-stereoscopic-depth-cameras). + + - The following filters have detailed descriptions in : https://github.com/IntelRealSense/librealsense/blob/master/doc/post-processing-filters.md + - ```disparity``` - convert depth to disparity before applying other filters and back. + - ```spatial``` - filter the depth image spatially. + - ```temporal``` - filter the depth image temporally. + - ```hole_filling``` - apply hole-filling filter. + - ```decimation``` - reduces depth scene complexity. +- **enable_sync**: gathers closest frames of different sensors, infra red, color and depth, to be sent with the same timetag. This happens automatically when such filters as pointcloud are enabled. +- ****_width**, ****_height**, ****_fps**: can be any of *infra, color, fisheye, depth, gyro, accel, pose, confidence*. Sets the required format of the device. If the specified combination of parameters is not available by the device, the stream will be replaced with the default for that stream. Setting a value to 0, will choose the first format in the inner list. (i.e. consistent between runs but not defined).
*Note: for gyro accel and pose, only _fps option is meaningful. +- **enable_****: Choose whether to enable a specified stream or not. Default is true for images and false for orientation streams. can be any of *infra1, infra2, color, depth, fisheye, fisheye1, fisheye2, gyro, accel, pose, confidence*. +- **tf_prefix**: By default all frame's ids have the same prefix - `camera_`. This allows changing it per camera. +- ****_frame_id**, ****_optical_frame_id**, **aligned_depth_to_**_frame_id**: Specify the different frame_id for the different frames. Especially important when using multiple cameras. +- **base_frame_id**: defines the frame_id all static transformations refers to. +- **odom_frame_id**: defines the origin coordinate system in ROS convention (X-Forward, Y-Left, Z-Up). pose topic defines the pose relative to that system. +- **All the rest of the frame_ids can be found in the template launch file: [nodelet.launch.xml](./realsense2_camera/launch/includes/nodelet.launch.xml)** +- **unite_imu_method**: The D435i and T265 cameras have built in IMU components which produce 2 unrelated streams: *gyro* - which shows angular velocity and *accel* which shows linear acceleration. Each with it's own frequency. By default, 2 corresponding topics are available, each with only the relevant fields of the message sensor_msgs::Imu are filled out. +Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default *gyro* and *accel* topics. The *imu* topic is published at the rate of the gyro. All the fields of the Imu message under the *imu* topic are filled out. + - **linear_interpolation**: Every gyro message is attached by the an accel message interpolated to the gyro's timestamp. + - **copy**: Every gyro message is attached by the last accel message. +- **clip_distance**: remove from the depth image all values above a given value (meters). Disable by giving negative value (default) +- **linear_accel_cov**, **angular_velocity_cov**: sets the variance given to the Imu readings. For the T265, these values are being modified by the inner confidence value. +- **hold_back_imu_for_frames**: Images processing takes time. Therefor there is a time gap between the moment the image arrives at the wrapper and the moment the image is published to the ROS environment. During this time, Imu messages keep on arriving and a situation is created where an image with earlier timestamp is published after Imu message with later timestamp. If that is a problem, setting *hold_back_imu_for_frames* to *true* will hold the Imu messages back while processing the images and then publish them all in a burst, thus keeping the order of publication as the order of arrival. Note that in either case, the timestamp in each message's header reflects the time of it's origin. +- **topic_odom_in**: For T265, add wheel odometry information through this topic. The code refers only to the *twist.linear* field in the message. +- **calib_odom_file**: For the T265 to include odometry input, it must be given a [configuration file](https://github.com/IntelRealSense/librealsense/blob/master/unit-tests/resources/calibration_odometry.json). Explanations can be found [here](https://github.com/IntelRealSense/librealsense/pull/3462). The calibration is done in ROS coordinates system. +- **publish_tf**: boolean, publish or not TF at all. Defaults to True. +- **tf_publish_rate**: double, positive values mean dynamic transform publication with specified rate, all other values mean static transform publication. Defaults to 0 +- **publish_odom_tf**: If True (default) publish TF from odom_frame to pose_frame. +- **infra_rgb**: When set to True (default: False), it configures the infrared camera to stream in RGB (color) mode, thus enabling the use of a RGB image in the same frame as the depth image, potentially avoiding frame transformation related errors. When this feature is required, you are additionally required to also enable `enable_infra:=true` for the infrared stream to be enabled. + - **NOTE** The configuration required for `enable_infra` is independent of `enable_depth` + - **NOTE** To enable the Infrared stream, you should enable `enable_infra:=true` NOT `enable_infra1:=true` nor `enable_infra2:=true` + - **NOTE** This feature is only supported by Realsense sensors with RGB streams available from the `infra` cameras, which can be checked by observing the output of `rs-enumerate-devices` + +### Available services: +- reset : Cause a hardware reset of the device. Usage: `rosservice call /camera/realsense2_camera/reset` +- enable : Start/Stop all streaming sensors. Usage example: `rosservice call /camera/enable False"` +- device_info : retrieve information about the device - serial_number, firmware_version etc. Type `osservice type /camera/realsense2_camera/device_info | rossrv show` for the full list. Call example: `rosservice call /camera/realsense2_camera/device_info` + +### Point Cloud +Here is an example of how to start the camera node and make it publish the point cloud using the pointcloud option. +```bash +roslaunch realsense2_camera rs_camera.launch filters:=pointcloud +``` +Then open rviz to watch the pointcloud: +

+ +### Aligned Depth Frames +Here is an example of how to start the camera node and make it publish the aligned depth stream to other available streams such as color or infra-red. +```bash +roslaunch realsense2_camera rs_camera.launch align_depth:=true +``` +

+ +### Set Camera Controls Using Dynamic Reconfigure Params +The following command allow to change camera control values using [http://wiki.ros.org/rqt_reconfigure]. +```bash +rosrun rqt_reconfigure rqt_reconfigure +``` +

+ +### Work with multiple cameras +**Important Notice:** Launching multiple T265 cameras is currently not supported. This will be addressed in a later version. + +Here is an example of how to start the camera node and streaming with two cameras using the [rs_multiple_devices.launch](./realsense2_camera/launch/rs_multiple_devices.launch). +```bash +roslaunch realsense2_camera rs_multiple_devices.launch serial_no_camera1:= serial_no_camera2:= +``` +The camera serial number should be provided to `serial_no_camera1` and `serial_no_camera2` parameters. One way to get the serial number is from the [rs-enumerate-devices](https://github.com/IntelRealSense/librealsense/blob/58d99783cc2781b1026eeed959aa3f7b562b20ca/tools/enumerate-devices/readme.md) tool. +```bash +rs-enumerate-devices | grep Serial +``` + +Another way of obtaining the serial number is connecting the camera alone, running +```bash +roslaunch realsense2_camera rs_camera.launch +``` +and looking for the serial number in the log printed to screen under "[INFO][...]Device Serial No:". + +Another way to use multiple cameras is running each from a different terminal. Make sure you set a different namespace for each camera using the "camera" argument: + +```bash +roslaunch realsense2_camera rs_camera.launch camera:=cam_1 serial_no:= +roslaunch realsense2_camera rs_camera.launch camera:=cam_2 serial_no:= +... + +``` +## Using T265 ## + +### Start the camera node +To start the camera node in ROS: + +```bash +roslaunch realsense2_camera rs_t265.launch +``` + +This will stream all camera sensors and publish on the appropriate ROS topics. + +The T265 sets its usb unique ID during initialization and without this parameter it wont be found. +Once running it will publish, among others, the following topics: +- /camera/odom/sample +- /camera/accel/sample +- /camera/gyro/sample +- /camera/fisheye1/image_raw +- /camera/fisheye2/image_raw + +To visualize the pose output and frames in RViz, start: +```bash +roslaunch realsense2_camera demo_t265.launch +``` + +### About Frame ID +The wrapper publishes static transformations(TFs). The Frame Ids are divided into 3 groups: +- ROS convention frames: follow the format of \_<\_stream>"\_frame" for example: camera_depth_frame, camera_infra1_frame, etc. +- Original frame coordinate system: with the suffix of <\_optical_frame>. For example: camera_infra1_optical_frame. Check the device documentation for specific coordinate system for each stream. +- base_link: For example: camera_link. A reference frame for the device. In D400 series and SR300 it is the depth frame. In T265, the pose frame. + + +### realsense2_description package: +For viewing included models, a separate package is included. For example: +```bash +roslaunch realsense2_description view_d415_model.launch +``` + +### Unit tests: +Unit-tests are based on bag files saved on S3 server. These can be downloaded using the following commands: +```bash +cd catkin_ws +wget "https://librealsense.intel.com/rs-tests/TestData/outdoors.bag" -P "records/" +wget "https://librealsense.intel.com/rs-tests/D435i_Depth_and_IMU_Stands_still.bag" -P "records/" +``` +Then, unit-tests can be run using the following command (use either python or python3): +```bash +python src/realsense/realsense2_camera/scripts/rs2_test.py --all +``` + +## Packages using RealSense ROS Camera +| Title | Links | +| ----- | ----- | +| ROS Object Analytics | [github](https://github.com/intel/ros_object_analytics) / [ROS Wiki](http://wiki.ros.org/IntelROSProject) + +## Known Issues +* This ROS node does not currently support [ROS Lunar Loggerhead](http://wiki.ros.org/lunar). +* This ROS node currently does not support running multiple T265 cameras at once. This will be addressed in a future update. + +## License +Copyright 2018 Intel Corporation + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this project except in compliance with the License. +You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. + +**Other names and brands may be claimed as the property of others* diff --git a/camera_ws/src/realsense-ros/realsense2_camera/include/base_realsense_node.h b/camera_ws/src/realsense-ros/realsense2_camera/include/base_realsense_node.h new file mode 100644 index 0000000000000000000000000000000000000000..750932275a9391acbcf112ea8daaa652d2e46fcd --- /dev/null +++ b/camera_ws/src/realsense-ros/realsense2_camera/include/base_realsense_node.h @@ -0,0 +1,345 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2018 Intel Corporation. All Rights Reserved + +#pragma once + +#include "../include/realsense_node_factory.h" +#include +#include "realsense2_camera/Metadata.h" +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace realsense2_camera +{ + struct FrequencyDiagnostics + { + FrequencyDiagnostics(double expected_frequency, std::string name, std::string hardware_id) : + expected_frequency_(expected_frequency), + frequency_status_(diagnostic_updater::FrequencyStatusParam(&expected_frequency_, &expected_frequency_)), + diagnostic_updater_(ros::NodeHandle(), ros::NodeHandle("~"), ros::this_node::getName() + "_" + name) + { + ROS_INFO("Expected frequency for %s = %.5f", name.c_str(), expected_frequency_); + diagnostic_updater_.setHardwareID(hardware_id); + diagnostic_updater_.add(frequency_status_); + } + + void tick() + { + frequency_status_.tick(); + } + + void update() + { + diagnostic_updater_.update(); + } + + double expected_frequency_; + diagnostic_updater::FrequencyStatus frequency_status_; + diagnostic_updater::Updater diagnostic_updater_; + }; + typedef std::pair> ImagePublisherWithFrequencyDiagnostics; + + class TemperatureDiagnostics + { + public: + TemperatureDiagnostics(std::string name, std::string serial_no); + void diagnostics(diagnostic_updater::DiagnosticStatusWrapper& status); + + void update(double crnt_temperaure) + { + _crnt_temp = crnt_temperaure; + _updater.update(); + } + + private: + double _crnt_temp; + diagnostic_updater::Updater _updater; + + }; + + class NamedFilter + { + public: + std::string _name; + std::shared_ptr _filter; + + public: + NamedFilter(std::string name, std::shared_ptr filter): + _name(name), _filter(filter) + {} + }; + + class PipelineSyncer : public rs2::asynchronous_syncer + { + public: + void operator()(rs2::frame f) const + { + invoke(std::move(f)); + } + }; + + class SyncedImuPublisher + { + public: + SyncedImuPublisher() {_is_enabled=false;}; + SyncedImuPublisher(ros::Publisher imu_publisher, std::size_t waiting_list_size=1000); + ~SyncedImuPublisher(); + void Pause(); // Pause sending messages. All messages from now on are saved in queue. + void Resume(); // Send all pending messages and allow sending future messages. + void Publish(sensor_msgs::Imu msg); //either send or hold message. + uint32_t getNumSubscribers() { return _publisher.getNumSubscribers();}; + void Enable(bool is_enabled) {_is_enabled=is_enabled;}; + + private: + void PublishPendingMessages(); + + private: + std::mutex _mutex; + ros::Publisher _publisher; + bool _pause_mode; + std::queue _pending_messages; + std::size_t _waiting_list_size; + bool _is_enabled; + }; + + class BaseRealSenseNode : public InterfaceRealSenseNode + { + public: + BaseRealSenseNode(ros::NodeHandle& nodeHandle, + ros::NodeHandle& privateNodeHandle, + rs2::device dev, + const std::string& serial_no); + + virtual void toggleSensors(bool enabled) override; + virtual void publishTopics() override; + virtual void registerDynamicReconfigCb(ros::NodeHandle& nh) override; + virtual ~BaseRealSenseNode(); + + public: + enum imu_sync_method{NONE, COPY, LINEAR_INTERPOLATION}; + + protected: + class float3 + { + public: + float x, y, z; + + public: + float3& operator*=(const float& factor) + { + x*=factor; + y*=factor; + z*=factor; + return (*this); + } + float3& operator+=(const float3& other) + { + x+=other.x; + y+=other.y; + z+=other.z; + return (*this); + } + }; + + bool _is_running; + std::string _base_frame_id; + std::string _odom_frame_id; + std::map _frame_id; + std::map _optical_frame_id; + std::map _depth_aligned_frame_id; + ros::NodeHandle& _node_handle, _pnh; + bool _align_depth; + std::vector _monitor_options; + std::shared_ptr _device_info_srv; + + virtual void calcAndPublishStaticTransform(const stream_index_pair& stream, const rs2::stream_profile& base_profile); + bool getDeviceInfo(realsense2_camera::DeviceInfo::Request& req, + realsense2_camera::DeviceInfo::Response& res); + rs2::stream_profile getAProfile(const stream_index_pair& stream); + tf::Quaternion rotationMatrixToQuaternion(const float rotation[9]) const; + void publish_static_tf(const ros::Time& t, + const float3& trans, + const tf::Quaternion& q, + const std::string& from, + const std::string& to); + + + private: + class CimuData + { + public: + CimuData() : m_time(-1) {}; + CimuData(const stream_index_pair type, Eigen::Vector3d data, double time): + m_type(type), + m_data(data), + m_time(time){}; + bool is_set() {return m_time > 0;}; + public: + stream_index_pair m_type; + Eigen::Vector3d m_data; + double m_time; + }; + + static std::string getNamespaceStr(); + void getParameters(); + void setupDevice(); + void setupErrorCallback(); + void setupPublishers(); + void enable_devices(); + void setupFilters(); + void setupStreams(); + bool setBaseTime(double frame_time, rs2_timestamp_domain time_domain); + double frameSystemTimeSec(rs2::frame frame); + cv::Mat& fix_depth_scale(const cv::Mat& from_image, cv::Mat& to_image); + void clip_depth(rs2::depth_frame depth_frame, float clipping_dist); + void updateStreamCalibData(const rs2::video_stream_profile& video_profile); + void SetBaseStream(); + void publishStaticTransforms(); + void publishDynamicTransforms(); + void publishIntrinsics(); + void runFirstFrameInitialization(rs2_stream stream_type); + void publishPointCloud(rs2::points f, const ros::Time& t, const rs2::frameset& frameset); + Extrinsics rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics, const std::string& frame_id) const; + + IMUInfo getImuInfo(const stream_index_pair& stream_index); + void publishFrame(rs2::frame f, const ros::Time& t, + const stream_index_pair& stream, + std::map& images, + const std::map& info_publishers, + const std::map& image_publishers, + const bool is_publishMetadata, + std::map& seq, + std::map& camera_info, + const std::map& encoding, + bool copy_data_from_frame = true); + void publishMetadata(rs2::frame f, const std::string& frame_id); + bool getEnabledProfile(const stream_index_pair& stream_index, rs2::stream_profile& profile); + + void publishAlignedDepthToOthers(rs2::frameset frames, const ros::Time& t); + sensor_msgs::Imu CreateUnitedMessage(const CimuData accel_data, const CimuData gyro_data); + + void FillImuData_Copy(const CimuData imu_data, std::deque& imu_msgs); + void ImuMessage_AddDefaultValues(sensor_msgs::Imu& imu_msg); + void FillImuData_LinearInterpolation(const CimuData imu_data, std::deque& imu_msgs); + void imu_callback(rs2::frame frame); + void imu_callback_sync(rs2::frame frame, imu_sync_method sync_method=imu_sync_method::COPY); + void pose_callback(rs2::frame frame); + void multiple_message_callback(rs2::frame frame, imu_sync_method sync_method); + void frame_callback(rs2::frame frame); + void registerDynamicOption(ros::NodeHandle& nh, rs2::options sensor, std::string& module_name); + void registerHDRoptions(); + void set_sensor_parameter_to_ros(const std::string& module_name, rs2::options sensor, rs2_option option); + void monitor_update_functions(); + void readAndSetDynamicParam(ros::NodeHandle& nh1, std::shared_ptr ddynrec, const std::string option_name, const int min_val, const int max_val, rs2::sensor sensor, int* option_value); + void registerAutoExposureROIOptions(ros::NodeHandle& nh); + void set_auto_exposure_roi(const std::string option_name, rs2::sensor sensor, int new_value); + void set_sensor_auto_exposure_roi(rs2::sensor sensor); + rs2_stream rs2_string_to_stream(std::string str); + void startMonitoring(); + void publish_temperature(); + void publish_frequency_update(); + void publishServices(); + + rs2::device _dev; + std::map _sensors; + std::map> _sensors_callback; + std::vector> _ddynrec; + + std::string _json_file_path; + std::string _serial_no; + float _depth_scale_meters; + float _clipping_distance; + bool _allow_no_texture_points; + bool _ordered_pc; + + + double _linear_accel_cov; + double _angular_velocity_cov; + bool _hold_back_imu_for_frames; + + std::map _stream_intrinsics; + std::map _width; + std::map _height; + std::map _fps; + std::map _format; + std::map _enable; + std::map _stream_name; + bool _publish_tf; + double _tf_publish_rate; + tf2_ros::StaticTransformBroadcaster _static_tf_broadcaster; + tf2_ros::TransformBroadcaster _dynamic_tf_broadcaster; + std::vector _static_tf_msgs; + std::shared_ptr _tf_t, _update_functions_t; + + std::map _image_publishers; + std::map _imu_publishers; + std::shared_ptr _synced_imu_publisher; + std::map _image_format; + std::map _info_publisher; + std::map> _metadata_publishers; + std::map _image; + std::map _encoding; + + std::map _seq; + std::map _unit_step_size; + std::map _camera_info; + std::atomic_bool _is_initialized_time_base; + double _camera_time_base; + std::map> _enabled_profiles; + + ros::Publisher _pointcloud_publisher; + ros::Time _ros_time_base; + bool _sync_frames; + bool _pointcloud; + bool _publish_odom_tf; + imu_sync_method _imu_sync_method; + std::string _filters_str; + stream_index_pair _pointcloud_texture; + PipelineSyncer _syncer; + std::vector _filters; + std::shared_ptr _colorizer, _pointcloud_filter; + std::vector _dev_sensors; + + std::map _depth_aligned_image; + std::map _depth_scaled_image; + std::map _depth_aligned_encoding; + std::map _depth_aligned_camera_info; + std::map _depth_aligned_seq; + std::map _depth_aligned_info_publisher; + std::map _depth_aligned_image_publishers; + std::map _depth_to_other_extrinsics_publishers; + std::map _depth_to_other_extrinsics; + std::map _auto_exposure_roi; + std::map _is_first_frame; + std::map > > _video_functions_stack; + + typedef std::pair> OptionTemperatureDiag; + std::vector< OptionTemperatureDiag > _temperature_nodes; + std::shared_ptr _monitoring_t; + std::vector > _update_functions_v; + mutable std::condition_variable _cv_monitoring, _cv_tf, _update_functions_cv; + + stream_index_pair _base_stream; + const std::string _namespace; + + sensor_msgs::PointCloud2 _msg_pointcloud; + std::vector< unsigned int > _valid_pc_indices; + };//end class + +} + diff --git a/camera_ws/src/realsense-ros/realsense2_camera/include/constants.h b/camera_ws/src/realsense-ros/realsense2_camera/include/constants.h new file mode 100644 index 0000000000000000000000000000000000000000..3d31a7a4b819e2d760c353a1594afa7479ef043d --- /dev/null +++ b/camera_ws/src/realsense-ros/realsense2_camera/include/constants.h @@ -0,0 +1,100 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2018 Intel Corporation. All Rights Reserved + +#pragma once + +#include + +#define REALSENSE_ROS_MAJOR_VERSION 2 +#define REALSENSE_ROS_MINOR_VERSION 3 +#define REALSENSE_ROS_PATCH_VERSION 2 + +#define STRINGIFY(arg) #arg +#define VAR_ARG_STRING(arg) STRINGIFY(arg) +/* Return version in "X.Y.Z" format */ +#define REALSENSE_ROS_VERSION_STR (VAR_ARG_STRING(REALSENSE_ROS_MAJOR_VERSION.REALSENSE_ROS_MINOR_VERSION.REALSENSE_ROS_PATCH_VERSION)) + +namespace realsense2_camera +{ + const uint16_t SR300_PID = 0x0aa5; // SR300 + const uint16_t SR300v2_PID = 0x0B48; // SR305 + const uint16_t RS400_PID = 0x0ad1; // PSR + const uint16_t RS410_PID = 0x0ad2; // ASR + const uint16_t RS415_PID = 0x0ad3; // ASRC + const uint16_t RS430_PID = 0x0ad4; // AWG + const uint16_t RS430_MM_PID = 0x0ad5; // AWGT + const uint16_t RS_USB2_PID = 0x0ad6; // USB2 + const uint16_t RS420_PID = 0x0af6; // PWG + const uint16_t RS420_MM_PID = 0x0afe; // PWGT + const uint16_t RS410_MM_PID = 0x0aff; // ASR + const uint16_t RS400_MM_PID = 0x0b00; // PSR + const uint16_t RS430_MM_RGB_PID = 0x0b01; // AWGCT + const uint16_t RS460_PID = 0x0b03; // DS5U + const uint16_t RS435_RGB_PID = 0x0b07; // AWGC + const uint16_t RS435i_RGB_PID = 0x0B3A; // AWGC_MM + const uint16_t RS465_PID = 0x0b4d; // D465 + const uint16_t RS416_RGB_PID = 0x0B52; // F416 RGB + const uint16_t RS405_PID = 0x0b0c; // DS5U + const uint16_t RS455_PID = 0x0B5C; // D455 + const uint16_t RS_T265_PID = 0x0b37; // + const uint16_t RS_L515_PID_PRE_PRQ = 0x0B3D; // + const uint16_t RS_L515_PID = 0x0B64; // + const uint16_t RS_L535_PID = 0x0b68; + + + const bool ALIGN_DEPTH = false; + const bool POINTCLOUD = false; + const bool ALLOW_NO_TEXTURE_POINTS = false; + const bool ORDERED_POINTCLOUD = false; + const bool SYNC_FRAMES = false; + + const bool PUBLISH_TF = true; + const double TF_PUBLISH_RATE = 0; // Static transform + + const int IMAGE_WIDTH = 640; + const int IMAGE_HEIGHT = 480; + const int IMAGE_FPS = 30; + + const int IMU_FPS = 0; + + + const bool ENABLE_DEPTH = true; + const bool ENABLE_INFRA1 = true; + const bool ENABLE_INFRA2 = true; + const bool ENABLE_COLOR = true; + const bool ENABLE_FISHEYE = true; + const bool ENABLE_IMU = true; + const bool HOLD_BACK_IMU_FOR_FRAMES = false; + const bool PUBLISH_ODOM_TF = true; + + + const std::string DEFAULT_BASE_FRAME_ID = "camera_link"; + const std::string DEFAULT_ODOM_FRAME_ID = "odom_frame"; + const std::string DEFAULT_DEPTH_FRAME_ID = "camera_depth_frame"; + const std::string DEFAULT_INFRA1_FRAME_ID = "camera_infra1_frame"; + const std::string DEFAULT_INFRA2_FRAME_ID = "camera_infra2_frame"; + const std::string DEFAULT_COLOR_FRAME_ID = "camera_color_frame"; + const std::string DEFAULT_FISHEYE_FRAME_ID = "camera_fisheye_frame"; + const std::string DEFAULT_IMU_FRAME_ID = "camera_imu_frame"; + + const std::string DEFAULT_DEPTH_OPTICAL_FRAME_ID = "camera_depth_optical_frame"; + const std::string DEFAULT_INFRA1_OPTICAL_FRAME_ID = "camera_infra1_optical_frame"; + const std::string DEFAULT_INFRA2_OPTICAL_FRAME_ID = "camera_infra2_optical_frame"; + const std::string DEFAULT_COLOR_OPTICAL_FRAME_ID = "camera_color_optical_frame"; + const std::string DEFAULT_FISHEYE_OPTICAL_FRAME_ID = "camera_fisheye_optical_frame"; + const std::string DEFAULT_ACCEL_OPTICAL_FRAME_ID = "camera_accel_optical_frame"; + const std::string DEFAULT_GYRO_OPTICAL_FRAME_ID = "camera_gyro_optical_frame"; + const std::string DEFAULT_IMU_OPTICAL_FRAME_ID = "camera_imu_optical_frame"; + + const std::string DEFAULT_ALIGNED_DEPTH_TO_COLOR_FRAME_ID = "camera_aligned_depth_to_color_frame"; + const std::string DEFAULT_ALIGNED_DEPTH_TO_INFRA1_FRAME_ID = "camera_aligned_depth_to_infra1_frame"; + const std::string DEFAULT_ALIGNED_DEPTH_TO_INFRA2_FRAME_ID = "camera_aligned_depth_to_infra2_frame"; + const std::string DEFAULT_ALIGNED_DEPTH_TO_FISHEYE_FRAME_ID = "camera_aligned_depth_to_fisheye_frame"; + + const std::string DEFAULT_UNITE_IMU_METHOD = ""; + const std::string DEFAULT_FILTERS = ""; + const std::string DEFAULT_TOPIC_ODOM_IN = ""; + + const float ROS_DEPTH_SCALE = 0.001; + using stream_index_pair = std::pair; +} // namespace realsense2_camera diff --git a/camera_ws/src/realsense-ros/realsense2_camera/include/realsense_node_factory.h b/camera_ws/src/realsense-ros/realsense2_camera/include/realsense_node_factory.h new file mode 100644 index 0000000000000000000000000000000000000000..921b43037f052c3b5427c475ec5fbdfc9dc7b484 --- /dev/null +++ b/camera_ws/src/realsense-ros/realsense2_camera/include/realsense_node_factory.h @@ -0,0 +1,90 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2018 Intel Corporation. All Rights Reserved + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace realsense2_camera +{ + const stream_index_pair COLOR{RS2_STREAM_COLOR, 0}; + const stream_index_pair DEPTH{RS2_STREAM_DEPTH, 0}; + const stream_index_pair INFRA0{RS2_STREAM_INFRARED, 0}; + const stream_index_pair INFRA1{RS2_STREAM_INFRARED, 1}; + const stream_index_pair INFRA2{RS2_STREAM_INFRARED, 2}; + const stream_index_pair FISHEYE{RS2_STREAM_FISHEYE, 0}; + const stream_index_pair FISHEYE1{RS2_STREAM_FISHEYE, 1}; + const stream_index_pair FISHEYE2{RS2_STREAM_FISHEYE, 2}; + const stream_index_pair GYRO{RS2_STREAM_GYRO, 0}; + const stream_index_pair ACCEL{RS2_STREAM_ACCEL, 0}; + const stream_index_pair POSE{RS2_STREAM_POSE, 0}; + const stream_index_pair CONFIDENCE{RS2_STREAM_CONFIDENCE, 0}; + + const std::vector IMAGE_STREAMS = {DEPTH, INFRA0, INFRA1, INFRA2, + COLOR, + FISHEYE, + FISHEYE1, FISHEYE2, CONFIDENCE}; + + const std::vector HID_STREAMS = {GYRO, ACCEL, POSE}; + + class InterfaceRealSenseNode + { + public: + virtual void publishTopics() = 0; + virtual void toggleSensors(bool enabled) = 0; + virtual void registerDynamicReconfigCb(ros::NodeHandle& nh) = 0; + virtual ~InterfaceRealSenseNode() = default; + }; + + class RealSenseNodeFactory : public nodelet::Nodelet + { + public: + RealSenseNodeFactory(); + virtual ~RealSenseNodeFactory(); + + private: + void closeDevice(); + void StartDevice(); + void change_device_callback(rs2::event_information& info); + void getDevice(rs2::device_list list); + virtual void onInit() override; + void initialize(const ros::WallTimerEvent &ignored); + void tryGetLogSeverity(rs2_log_severity& severity) const; + void reset(); + bool handleReset(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); + static std::string parse_usb_port(std::string line); + bool toggle_sensor_callback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res); + + rs2::device _device; + std::shared_ptr _realSenseNode; + rs2::context _ctx; + std::string _serial_no; + std::string _usb_port_id; + std::string _device_type; + bool _initial_reset; + std::thread _query_thread; + bool _is_alive; + ros::ServiceServer toggle_sensor_srv; + ros::WallTimer _init_timer; + ros::ServiceServer _reset_srv; + + }; +}//end namespace diff --git a/camera_ws/src/realsense-ros/realsense2_camera/launch/demo_pointcloud.launch b/camera_ws/src/realsense-ros/realsense2_camera/launch/demo_pointcloud.launch new file mode 100644 index 0000000000000000000000000000000000000000..18084c99a3da9ec6c6d5430dc1fdf5dd605cc53b --- /dev/null +++ b/camera_ws/src/realsense-ros/realsense2_camera/launch/demo_pointcloud.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/camera_ws/src/realsense-ros/realsense2_camera/lib/libcout_serial_num.so b/camera_ws/src/realsense-ros/realsense2_camera/lib/libcout_serial_num.so new file mode 100644 index 0000000000000000000000000000000000000000..ee22034ec222b5911e1dee9b8b24a6aa2912951a Binary files /dev/null and b/camera_ws/src/realsense-ros/realsense2_camera/lib/libcout_serial_num.so differ diff --git a/camera_ws/src/realsense-ros/realsense2_camera/librealsense2_bionic.rdmanifest b/camera_ws/src/realsense-ros/realsense2_camera/librealsense2_bionic.rdmanifest new file mode 100644 index 0000000000000000000000000000000000000000..092c454a88f828b22fe89003dc68427d9a5af649 --- /dev/null +++ b/camera_ws/src/realsense-ros/realsense2_camera/librealsense2_bionic.rdmanifest @@ -0,0 +1,37 @@ +uri: 'https://raw.githubusercontent.com/magazino/pylon_camera/indigo-devel/rosdep/empty.tar' +check-presence-script: | + #!/bin/bash + + if [ $(dpkg-query -W -f='${Status}' librealsense2-dkms 2>/dev/null | grep -c "ok installed") -eq 0 ]; + then + exit 1 + else + exit 0 + fi + + +install-script: | + #!/bin/bash + + # Install + if [[ "$EUID" -ne 0 ]]; then + # Install add-apt-repository + sudo apt-get install -y software-properties-common + # Register the server's public key + sudo apt-key adv --keyserver keys.gnupg.net --recv-key C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C8B3A55A6F3EFCDE + # Add the server to the list of repositories + sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo bionic main" -u + # Install the libraries + sudo apt-get install -y librealsense2-dkms librealsense2-dev + else + # Install add-apt-repository + apt-get install -y software-properties-common + # Register the server's public key + apt-key adv --keyserver keys.gnupg.net --recv-key C8B3A55A6F3EFCDE || apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C8B3A55A6F3EFCDE + # Add the server to the list of repositories + add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo bionic main" -u + # Install the libraries + apt-get install -y librealsense2-dkms librealsense2-dev + fi + + exit $? \ No newline at end of file diff --git a/camera_ws/src/realsense-ros/realsense2_camera/librealsense2_xenial.rdmanifest b/camera_ws/src/realsense-ros/realsense2_camera/librealsense2_xenial.rdmanifest new file mode 100644 index 0000000000000000000000000000000000000000..68f129899c33dcac19e57dfd8886286710b9c9c0 --- /dev/null +++ b/camera_ws/src/realsense-ros/realsense2_camera/librealsense2_xenial.rdmanifest @@ -0,0 +1,37 @@ +uri: 'https://raw.githubusercontent.com/magazino/pylon_camera/indigo-devel/rosdep/empty.tar' +check-presence-script: | + #!/bin/bash + + if [ $(dpkg-query -W -f='${Status}' librealsense2-dkms 2>/dev/null | grep -c "ok installed") -eq 0 ]; + then + exit 1 + else + exit 0 + fi + + +install-script: | + #!/bin/bash + + # Install + if [[ "$EUID" -ne 0 ]]; then + # Install add-apt-repository + sudo apt-get install -y software-properties-common + # Register the server's public key + sudo apt-key adv --keyserver keys.gnupg.net --recv-key C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C8B3A55A6F3EFCDE + # Add the server to the list of repositories + sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo xenial main" -u + # Install the libraries + sudo apt-get install -y librealsense2-dkms librealsense2-dev + else + # Install add-apt-repository + apt-get install -y software-properties-common + # Register the server's public key + apt-key adv --keyserver keys.gnupg.net --recv-key C8B3A55A6F3EFCDE || apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C8B3A55A6F3EFCDE + # Add the server to the list of repositories + add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo xenial main" -u + # Install the libraries + apt-get install -y librealsense2-dkms librealsense2-dev + fi + + exit $? \ No newline at end of file diff --git a/camera_ws/src/realsense-ros/realsense2_camera/nodelet_plugins.xml b/camera_ws/src/realsense-ros/realsense2_camera/nodelet_plugins.xml new file mode 100644 index 0000000000000000000000000000000000000000..838d23a53eb28bcf4f3caba4592f1b2e424a718e --- /dev/null +++ b/camera_ws/src/realsense-ros/realsense2_camera/nodelet_plugins.xml @@ -0,0 +1,7 @@ + + + + Example camera nodelet using the Intel RealSense SDK 2.0 library for Intel RealSense SR300 and D400 cameras + + + diff --git a/camera_ws/src/realsense-ros/realsense2_camera/scripts/rs2_test.py b/camera_ws/src/realsense-ros/realsense2_camera/scripts/rs2_test.py new file mode 100644 index 0000000000000000000000000000000000000000..9498bd8cb048652ee23669122b73e732eb1ee742 --- /dev/null +++ b/camera_ws/src/realsense-ros/realsense2_camera/scripts/rs2_test.py @@ -0,0 +1,385 @@ +import os +import sys +from rs2_listener import CWaitForMessage + +import rosbag +from cv_bridge import CvBridge, CvBridgeError +import numpy as np +import tf +import itertools +import subprocess +import rospy +import time +import rosservice + +global tf_timeout +tf_timeout = 5 + +def ImuGetData(rec_filename, topic): + # res['value'] = first value of topic. + # res['max_diff'] = max difference between returned value and all other values of topic in recording. + + bag = rosbag.Bag(rec_filename) + res = dict() + res['value'] = None + res['max_diff'] = [0,0,0] + for topic, msg, t in bag.read_messages(topics=topic): + value = np.array([msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z]) + if res['value'] is None: + res['value'] = value + else: + diff = abs(value - res['value']) + res['max_diff'] = [max(diff[x], res['max_diff'][x]) for x in range(len(diff))] + res['max_diff'] = np.array(res['max_diff']) + return res + +def AccelGetData(rec_filename): + return ImuGetData(rec_filename, '/device_0/sensor_2/Accel_0/imu/data') + +def AccelGetDataDeviceStandStraight(rec_filename): + gt_data = AccelGetData(rec_filename) + gt_data['ros_value'] = np.array([0.63839424, 0.05380408, 9.85343552]) + gt_data['ros_max_diff'] = np.array([1.97013582e-02, 4.65862500e-09, 4.06165277e-02]) + return gt_data + +def ImuTest(data, gt_data): + # check that the imu data received is the same as in the recording. + # check that in the rotated imu received the g-accelartation is pointing up according to ROS standards. + try: + v_data = np.array([data['value'][0].x, data['value'][0].y, data['value'][0].z]) + v_gt_data = gt_data['value'] + diff = v_data - v_gt_data + max_diff = abs(diff).max() + msg = 'original accel: Expect max diff of %.3f. Got %.3f.' % (gt_data['max_diff'].max(), max_diff) + print (msg) + if max_diff > gt_data['max_diff'].max(): + return False, msg + + v_data = data['ros_value'][0] + v_gt_data = gt_data['ros_value'] + diff = v_data - v_gt_data + max_diff = abs(diff).max() + msg = 'rotated to ROS: Expect max diff of %.3f. Got %.3f.' % (gt_data['ros_max_diff'].max(), max_diff) + print (msg) + if max_diff > gt_data['ros_max_diff'].max(): + return False, msg + except Exception as e: + msg = '%s' % e + print ('Test Failed: %s' % msg) + return False, msg + return True, '' + +def ImageGetData(rec_filename, topic): + bag = rosbag.Bag(rec_filename) + bridge = CvBridge() + all_avg = [] + ok_percent = [] + res = dict() + + for topic, msg, t in bag.read_messages(topics=topic): + try: + cv_image = bridge.imgmsg_to_cv2(msg, msg.encoding) + except CvBridgeError as e: + print(e) + continue + pyimg = np.asarray(cv_image) + ok_number = (pyimg != 0).sum() + ok_percent.append(float(ok_number) / (pyimg.shape[0] * pyimg.shape[1])) + all_avg.append(pyimg.sum() / ok_number) + + all_avg = np.array(all_avg) + channels = cv_image.shape[2] if len(cv_image.shape) > 2 else 1 + res['num_channels'] = channels + res['shape'] = cv_image.shape + res['avg'] = all_avg.mean() + res['ok_percent'] = {'value': (np.array(ok_percent).mean()) / channels, 'epsilon': 0.01} + res['epsilon'] = max(all_avg.max() - res['avg'], res['avg'] - all_avg.min()) + res['reported_size'] = [msg.width, msg.height, msg.step] + + return res + + +def ImageColorGetData(rec_filename): + return ImageGetData(rec_filename, '/device_0/sensor_1/Color_0/image/data') + + +def ImageDepthGetData(rec_filename): + return ImageGetData(rec_filename, '/device_0/sensor_0/Depth_0/image/data') + + +def ImageDepthInColorShapeGetData(rec_filename): + gt_data = ImageDepthGetData(rec_filename) + color_data = ImageColorGetData(rec_filename) + gt_data['shape'] = color_data['shape'][:2] + gt_data['reported_size'] = color_data['reported_size'] + gt_data['reported_size'][2] = gt_data['reported_size'][0]*2 + gt_data['ok_percent']['epsilon'] *= 3 + return gt_data + +def ImageDepthGetData_decimation(rec_filename): + gt_data = ImageDepthGetData(rec_filename) + gt_data['shape'] = [x/2 for x in gt_data['shape']] + gt_data['reported_size'] = [x/2 for x in gt_data['reported_size']] + gt_data['epsilon'] *= 3 + return gt_data + +def ImageColorTest(data, gt_data): + # check that all data['num_channels'] are the same as gt_data['num_channels'] and that avg value of all + # images are within epsilon of gt_data['avg'] + try: + channels = list(set(data['num_channels'])) + msg = 'Expect %d channels. Got %d channels.' % (gt_data['num_channels'], channels[0]) + print (msg) + if len(channels) > 1 or channels[0] != gt_data['num_channels']: + return False, msg + msg = 'Expected all received images to be the same shape. Got %s' % str(set(data['shape'])) + print (msg) + if len(set(data['shape'])) > 1: + return False, msg + msg = 'Expected shape to be %s. Got %s' % (gt_data['shape'], list(set(data['shape']))[0]) + print (msg) + if (np.array(list(set(data['shape']))[0]) != np.array(gt_data['shape'])).any(): + return False, msg + msg = 'Expected header [width, height, step] to be %s. Got %s' % (gt_data['reported_size'], list(set(data['reported_size']))[0]) + print (msg) + if (np.array(list(set(data['reported_size']))[0]) != np.array(gt_data['reported_size'])).any(): + return False, msg + msg = 'Expect average of %.3f (+-%.3f). Got average of %.3f.' % (gt_data['avg'].mean(), gt_data['epsilon'], np.array(data['avg']).mean()) + print (msg) + if abs(np.array(data['avg']).mean() - gt_data['avg'].mean()) > gt_data['epsilon']: + return False, msg + + msg = 'Expect no holes percent > %.3f. Got %.3f.' % (gt_data['ok_percent']['value']-gt_data['ok_percent']['epsilon'], np.array(data['ok_percent']).mean()) + print (msg) + if np.array(data['ok_percent']).mean() < gt_data['ok_percent']['value']-gt_data['ok_percent']['epsilon']: + return False, msg + + except Exception as e: + msg = '%s' % e + print ('Test Failed: %s' % msg) + return False, msg + return True, '' + + +def ImageColorTest_3epsilon(data, gt_data): + gt_data['epsilon'] *= 3 + return ImageColorTest(data, gt_data) + +def NotImageColorTest(data, gt_data): + res = ImageColorTest(data, gt_data) + return (not res[0], res[1]) + +def PointCloudTest(data, gt_data): + width = np.array(data['width']).mean() + height = np.array(data['height']).mean() + msg = 'Expect image size %d(+-%d), %d. Got %d, %d.' % (gt_data['width'][0], gt_data['width'][1], gt_data['height'][0], width, height) + print (msg) + if abs(width - gt_data['width'][0]) > gt_data['width'][1] or height != gt_data['height'][0]: + return False, msg + mean_pos = np.array([xx[:3] for xx in data['avg']]).mean(0) + msg = 'Expect average position of %s (+-%.3f). Got average of %s.' % (gt_data['avg'][0][:3], gt_data['epsilon'][0], mean_pos) + print (msg) + if abs(mean_pos - gt_data['avg'][0][:3]).max() > gt_data['epsilon'][0]: + return False, msg + mean_col = np.array([xx[3:] for xx in data['avg']]).mean(0) + msg = 'Expect average color of %s (+-%.3f). Got average of %s.' % (gt_data['avg'][0][3:], gt_data['epsilon'][1], mean_col) + print (msg) + if abs(mean_col - gt_data['avg'][0][3:]).max() > gt_data['epsilon'][1]: + return False, msg + + return True, '' + + +def staticTFTest(data, gt_data): + for couple in gt_data.keys(): + if data[couple] is None: + msg = 'Tf is None for couple %s' % '->'.join(couple) + return False, msg + if any(abs((np.array(data[couple][0]) - np.array(gt_data[couple][0]))) > 1e-5) or \ + any(abs((np.array(data[couple][1]) - np.array(gt_data[couple][1]))) > 1e-5): + msg = 'Tf is changed for couple %s' % '->'.join(couple) + return False, msg + return True, '' + +test_types = {'vis_avg': {'listener_theme': 'colorStream', + 'data_func': ImageColorGetData, + 'test_func': ImageColorTest}, + 'depth_avg': {'listener_theme': 'depthStream', + 'data_func': ImageDepthGetData, + 'test_func': ImageColorTest}, + 'no_file': {'listener_theme': 'colorStream', + 'data_func': lambda x: None, + 'test_func': NotImageColorTest}, + 'pointscloud_avg': {'listener_theme': 'pointscloud', + 'data_func': lambda x: {'width': [660353, 2300], 'height': [1], 'avg': [np.array([ 1.28251814, -0.15839984, 4.82235184, 80, 160, 240])], 'epsilon': [0.04, 5]}, + 'test_func': PointCloudTest}, + 'align_depth_ir1': {'listener_theme': 'alignedDepthInfra1', + 'data_func': ImageDepthGetData, + 'test_func': ImageColorTest}, + 'align_depth_color': {'listener_theme': 'alignedDepthColor', + 'data_func': ImageDepthInColorShapeGetData, + 'test_func': ImageColorTest_3epsilon}, + 'depth_avg_decimation': {'listener_theme': 'depthStream', + 'data_func': ImageDepthGetData_decimation, + 'test_func': ImageColorTest}, + 'align_depth_ir1_decimation': {'listener_theme': 'alignedDepthInfra1', + 'data_func': ImageDepthGetData, + 'test_func': ImageColorTest}, + 'static_tf': {'listener_theme': 'static_tf', + 'data_func': lambda x: {('camera_link', 'camera_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702]), + ('camera_link', 'camera_depth_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), + ('camera_link', 'camera_infra1_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), + ('camera_depth_frame', 'camera_infra1_frame'): ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]), + ('camera_depth_frame', 'camera_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702]), + ('camera_infra1_frame', 'camera_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702])} + , + 'test_func': staticTFTest}, + 'accel_up': {'listener_theme': 'accelStream', + 'data_func': AccelGetDataDeviceStandStraight, + 'test_func': ImuTest}, + } + + +def run_test(test, listener_res): + # gather ground truth with test_types[test['type']]['data_func'] and recording from test['rosbag_filename'] + # return results from test_types[test['type']]['test_func'] + test_type = test_types[test['type']] + gt_data = test_type['data_func'](test['params']['rosbag_filename']) + return test_type['test_func'](listener_res[test_type['listener_theme']], gt_data) + + +def print_results(results): + title = 'TEST RESULTS' + headers = ['index', 'test name', 'score', 'message'] + col_0_width = len(headers[0]) + 1 + col_1_width = max([len(headers[1])] + [len(test[0]) for test in results]) + 1 + col_2_width = max([len(headers[2]), len('OK'), len('FAILED')]) + 1 + col_3_width = max([len(headers[3])] + [len(test[1][1]) for test in results]) + 1 + total_width = col_0_width + col_1_width + col_2_width + col_3_width + print + print (('{:^%ds}'%total_width).format(title)) + print ('-'*total_width) + print (('{:<%ds}{:<%ds}{:>%ds} : {:<%ds}' % (col_0_width, col_1_width, col_2_width, col_3_width)).format(*headers)) + print ('-'*(col_0_width-1) + ' '*1 + '-'*(col_1_width-1) + ' '*2 + '-'*(col_2_width-1) + ' '*3 + '-'*(col_3_width-1)) + print ('\n'.join([('{:<%dd}{:<%ds}{:>%ds} : {: %s for %.2f(sec)' % (from_id, to_id, tf_timeout) + tf_listener.waitForTransform(from_id, to_id, rospy.Time(), rospy.Duration(tf_timeout)) + res = tf_listener.lookupTransform(from_id, to_id, rospy.Time()) + except Exception as e: + res = None + finally: + waited_for = time.time() - start_time + tf_timeout = max(0.0, tf_timeout - waited_for) + return res + + +def run_tests(tests): + msg_params = {'timeout_secs': 5} + results = [] + params_strs = set([test['params_str'] for test in tests]) + for params_str in params_strs: + rec_tests = [test for test in tests if test['params_str'] == params_str] + themes = [test_types[test['type']]['listener_theme'] for test in rec_tests] + msg_retriever = CWaitForMessage(msg_params) + print ('*'*30) + print ('Running the following tests: %s' % ('\n' + '\n'.join([test['name'] for test in rec_tests]))) + print ('*'*30) + num_of_startups = 5 + is_node_up = False + for run_no in range(num_of_startups): + print + print ('*'*8 + ' Starting ROS ' + '*'*8) + print ('running node (%d/%d)' % (run_no, num_of_startups)) + cmd_params = ['roslaunch', 'realsense2_camera', 'rs_from_file.launch'] + params_str.split(' ') + print ('running command: ' + ' '.join(cmd_params)) + p_wrapper = subprocess.Popen(cmd_params, stdout=None, stderr=None) + time.sleep(2) + service_list = rosservice.get_service_list() + is_node_up = len([service for service in service_list if 'realsense2_camera/' in service]) > 0 + if is_node_up: + print ('Node is UP') + break + print ('Node is NOT UP') + print ('*'*8 + ' Killing ROS ' + '*'*9) + p_wrapper.terminate() + p_wrapper.wait() + print ('DONE') + + if is_node_up: + listener_res = msg_retriever.wait_for_messages(themes) + if 'static_tf' in [test['type'] for test in rec_tests]: + print ('Gathering static transforms') + frame_ids = ['camera_link', 'camera_depth_frame', 'camera_infra1_frame', 'camera_infra2_frame', 'camera_color_frame', 'camera_fisheye_frame', 'camera_pose'] + tf_listener = tf.TransformListener() + listener_res['static_tf'] = dict([(xx, get_tf(tf_listener, xx[0], xx[1])) for xx in itertools.combinations(frame_ids, 2)]) + print ('*'*8 + ' Killing ROS ' + '*'*9) + p_wrapper.terminate() + p_wrapper.wait() + else: + listener_res = dict([[theme_name, {}] for theme_name in themes]) + + print ('*'*30) + print ('DONE run') + print ('*'*30) + + for test in rec_tests: + try: + res = run_test(test, listener_res) + except Exception as e: + print ('Test %s Failed: %s' % (test['name'], e)) + res = False, '%s' % e + results.append([test['name'], res]) + + return results + + +def main(): + outdoors_filename = './records/outdoors_1color.bag' + all_tests = [{'name': 'non_existent_file', 'type': 'no_file', 'params': {'rosbag_filename': '/home/non_existent_file.txt'}}, + {'name': 'vis_avg_2', 'type': 'vis_avg', 'params': {'rosbag_filename': outdoors_filename}}, + {'name': 'depth_avg_1', 'type': 'depth_avg', 'params': {'rosbag_filename': outdoors_filename}}, + {'name': 'depth_w_cloud_1', 'type': 'depth_avg', 'params': {'rosbag_filename': outdoors_filename, 'enable_pointcloud': 'true'}}, + # {'name': 'points_cloud_1', 'type': 'pointscloud_avg', 'params': {'rosbag_filename': outdoors_filename, 'enable_pointcloud': 'true'}}, + # {'name': 'align_depth_color_1', 'type': 'align_depth_color', 'params': {'rosbag_filename': outdoors_filename, 'align_depth': 'true'}}, + # {'name': 'align_depth_ir1_1', 'type': 'align_depth_ir1', 'params': {'rosbag_filename': outdoors_filename, 'align_depth': 'true'}}, + {'name': 'depth_avg_decimation_1', 'type': 'depth_avg_decimation', 'params': {'rosbag_filename': outdoors_filename, 'filters': 'decimation'}}, + # {'name': 'align_depth_ir1_decimation_1', 'type': 'align_depth_ir1_decimation', 'params': {'rosbag_filename': outdoors_filename, 'filters': 'decimation', 'align_depth': 'true'}}, + # {'name': 'static_tf_1', 'type': 'static_tf', 'params': {'rosbag_filename': outdoors_filename}}, # Not working in Travis... + # {'name': 'accel_up_1', 'type': 'accel_up', 'params': {'rosbag_filename': './records/D435i_Depth_and_IMU_Stands_still.bag'}}, # Keeps failing on Travis CI. See https://github.com/IntelRealSense/realsense-ros/pull/1504#issuecomment-744226704 + ] + + # Normalize parameters: + for test in all_tests: + test['params']['rosbag_filename'] = os.path.abspath(test['params']['rosbag_filename']) + test['params_str'] = ' '.join([key + ':=' + test['params'][key] for key in sorted(test['params'].keys())]) + + if len(sys.argv) < 2 or '--help' in sys.argv or '/?' in sys.argv: + print ('USAGE:') + print ('------') + print ('rs2_test.py --all | [ [...]]') + print + print ('Available tests are:') + print ('\n'.join([test['name'] for test in all_tests])) + exit(-1) + + if '--all' in sys.argv[1:]: + tests_to_run = all_tests + else: + tests_to_run = [test for test in all_tests if test['name'] in sys.argv[1:]] + + results = run_tests(tests_to_run) + print_results(results) + + res = int(all([result[1][0] for result in results])) - 1 + print ('exit (%d)' % res) + exit(res) + +if __name__ == '__main__': + main() diff --git a/camera_ws/src/realsense-ros/realsense2_camera/scripts/show_center_depth.py b/camera_ws/src/realsense-ros/realsense2_camera/scripts/show_center_depth.py new file mode 100644 index 0000000000000000000000000000000000000000..5722d4514dbe2192901d909c0830c6dff0940537 --- /dev/null +++ b/camera_ws/src/realsense-ros/realsense2_camera/scripts/show_center_depth.py @@ -0,0 +1,101 @@ +import rospy +from sensor_msgs.msg import Image as msg_Image +from sensor_msgs.msg import CameraInfo +from cv_bridge import CvBridge, CvBridgeError +import sys +import os +import numpy as np +import pyrealsense2 as rs2 +if (not hasattr(rs2, 'intrinsics')): + import pyrealsense2.pyrealsense2 as rs2 + +class ImageListener: + def __init__(self, depth_image_topic, depth_info_topic): + self.bridge = CvBridge() + self.sub = rospy.Subscriber(depth_image_topic, msg_Image, self.imageDepthCallback) + self.sub_info = rospy.Subscriber(depth_info_topic, CameraInfo, self.imageDepthInfoCallback) + confidence_topic = depth_image_topic.replace('depth', 'confidence') + self.sub_conf = rospy.Subscriber(confidence_topic, msg_Image, self.confidenceCallback) + self.intrinsics = None + self.pix = None + self.pix_grade = None + + def imageDepthCallback(self, data): + try: + cv_image = self.bridge.imgmsg_to_cv2(data, data.encoding) + # pick one pixel among all the pixels with the closest range: + indices = np.array(np.where(cv_image == cv_image[cv_image > 0].min()))[:,0] + pix = (indices[1], indices[0]) + self.pix = pix + line = '\rDepth at pixel(%3d, %3d): %7.1f(mm).' % (pix[0], pix[1], cv_image[pix[1], pix[0]]) + + if self.intrinsics: + depth = cv_image[pix[1], pix[0]] + result = rs2.rs2_deproject_pixel_to_point(self.intrinsics, [pix[0], pix[1]], depth) + line += ' Coordinate: %8.2f %8.2f %8.2f.' % (result[0], result[1], result[2]) + if (not self.pix_grade is None): + line += ' Grade: %2d' % self.pix_grade + line += '\r' + sys.stdout.write(line) + sys.stdout.flush() + + except CvBridgeError as e: + print(e) + return + except ValueError as e: + return + + def confidenceCallback(self, data): + try: + cv_image = self.bridge.imgmsg_to_cv2(data, data.encoding) + grades = np.bitwise_and(cv_image >> 4, 0x0f) + if (self.pix): + self.pix_grade = grades[self.pix[1], self.pix[0]] + except CvBridgeError as e: + print(e) + return + + + + def imageDepthInfoCallback(self, cameraInfo): + try: + if self.intrinsics: + return + self.intrinsics = rs2.intrinsics() + self.intrinsics.width = cameraInfo.width + self.intrinsics.height = cameraInfo.height + self.intrinsics.ppx = cameraInfo.K[2] + self.intrinsics.ppy = cameraInfo.K[5] + self.intrinsics.fx = cameraInfo.K[0] + self.intrinsics.fy = cameraInfo.K[4] + if cameraInfo.distortion_model == 'plumb_bob': + self.intrinsics.model = rs2.distortion.brown_conrady + elif cameraInfo.distortion_model == 'equidistant': + self.intrinsics.model = rs2.distortion.kannala_brandt4 + self.intrinsics.coeffs = [i for i in cameraInfo.D] + except CvBridgeError as e: + print(e) + return + +def main(): + depth_image_topic = '/camera/depth/image_rect_raw' + depth_info_topic = '/camera/depth/camera_info' + + print ('') + print ('show_center_depth.py') + print ('--------------------') + print ('App to demontrate the usage of the /camera/depth topics.') + print ('') + print ('Application subscribes to %s and %s topics.' % (depth_image_topic, depth_info_topic)) + print ('Application then calculates and print the range to the closest object.') + print ('If intrinsics data is available, it also prints the 3D location of the object') + print ('If a confedence map is also available in the topic %s, it also prints the confidence grade.' % depth_image_topic.replace('depth', 'confidence')) + print ('') + + listener = ImageListener(depth_image_topic, depth_info_topic) + rospy.spin() + +if __name__ == '__main__': + node_name = os.path.basename(sys.argv[0]).split('.')[0] + rospy.init_node(node_name) + main() diff --git a/camera_ws/src/realsense-ros/realsense2_camera/src/t265_realsense_node.cpp b/camera_ws/src/realsense-ros/realsense2_camera/src/t265_realsense_node.cpp new file mode 100644 index 0000000000000000000000000000000000000000..2a4e76db25bf11550e9cef328f1bdc9b4ccce30a --- /dev/null +++ b/camera_ws/src/realsense-ros/realsense2_camera/src/t265_realsense_node.cpp @@ -0,0 +1,146 @@ +#include "../include/t265_realsense_node.h" + +using namespace realsense2_camera; + +T265RealsenseNode::T265RealsenseNode(ros::NodeHandle& nodeHandle, + ros::NodeHandle& privateNodeHandle, + rs2::device dev, + const std::string& serial_no) : + BaseRealSenseNode(nodeHandle, privateNodeHandle, dev, serial_no), + _wo_snr(dev.first()), + _use_odom_in(false) + { + _monitor_options = {RS2_OPTION_ASIC_TEMPERATURE, RS2_OPTION_MOTION_MODULE_TEMPERATURE}; + initializeOdometryInput(); + handleWarning(); + } + +void T265RealsenseNode::initializeOdometryInput() +{ + std::string calib_odom_file; + _pnh.param("calib_odom_file", calib_odom_file, std::string("")); + if (calib_odom_file.empty()) + { + ROS_INFO("No calib_odom_file. No input odometry accepted."); + return; + } + std::ifstream calibrationFile(calib_odom_file); + if (!calibrationFile) + { + ROS_FATAL_STREAM("calibration_odometry file not found. calib_odom_file = " << calib_odom_file); + throw std::runtime_error("calibration_odometry file not found" ); + } + const std::string json_str((std::istreambuf_iterator(calibrationFile)), + std::istreambuf_iterator()); + const std::vector wo_calib(json_str.begin(), json_str.end()); + + if (!_wo_snr.load_wheel_odometery_config(wo_calib)) + { + ROS_FATAL_STREAM("Format error in calibration_odometry file: " << calib_odom_file); + throw std::runtime_error("Format error in calibration_odometry file" ); + } + _use_odom_in = true; +} + +void T265RealsenseNode::toggleSensors(bool enabled) +{ + ROS_WARN_STREAM("toggleSensors method not implemented for T265"); +} + +void T265RealsenseNode::publishTopics() +{ + BaseRealSenseNode::publishTopics(); + setupSubscribers(); +} + +void T265RealsenseNode::handleWarning() +{ + rs2::log_to_callback( rs2_log_severity::RS2_LOG_SEVERITY_WARN, [&] + ( rs2_log_severity severity, rs2::log_message const & msg ) noexcept { + _T265_fault = msg.raw(); + std::array list_of_fault{"SLAM_ERROR", "Stream transfer failed, exiting"}; + auto it = std::find_if(begin(list_of_fault), end(list_of_fault), + [&](const std::string& s) {return _T265_fault.find(s) != std::string::npos; }); + if (it != end(list_of_fault)) + { + callback_updater.add("Warning ",this, & T265RealsenseNode::warningDiagnostic); + callback_updater.force_update(); + } + }); +} + +void T265RealsenseNode::setupSubscribers() +{ + if (!_use_odom_in) return; + + std::string topic_odom_in; + _pnh.param("topic_odom_in", topic_odom_in, DEFAULT_TOPIC_ODOM_IN); + ROS_INFO_STREAM("Subscribing to in_odom topic: " << topic_odom_in); + + _odom_subscriber = _node_handle.subscribe(topic_odom_in, 1, &T265RealsenseNode::odom_in_callback, this); +} + +void T265RealsenseNode::odom_in_callback(const nav_msgs::Odometry::ConstPtr& msg) +{ + ROS_DEBUG("Got in_odom message"); + rs2_vector velocity {-(float)(msg->twist.twist.linear.y), + (float)(msg->twist.twist.linear.z), + -(float)(msg->twist.twist.linear.x)}; + + ROS_DEBUG_STREAM("Add odom: " << velocity.x << ", " << velocity.y << ", " << velocity.z); + _wo_snr.send_wheel_odometry(0, 0, velocity); +} + +void T265RealsenseNode::calcAndPublishStaticTransform(const stream_index_pair& stream, const rs2::stream_profile& base_profile) +{ + // Transform base to stream + tf::Quaternion quaternion_optical; + quaternion_optical.setRPY(M_PI / 2, 0.0, -M_PI / 2); //Pose To ROS + float3 zero_trans{0, 0, 0}; + + ros::Time transform_ts_ = ros::Time::now(); + + rs2_extrinsics ex; + try + { + ex = getAProfile(stream).get_extrinsics_to(base_profile); + } + catch (std::exception& e) + { + if (!strcmp(e.what(), "Requested extrinsics are not available!")) + { + ROS_WARN_STREAM(e.what() << " : using unity as default."); + ex = rs2_extrinsics({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0,0,0}}); + } + else + { + throw e; + } + } + + auto Q = rotationMatrixToQuaternion(ex.rotation); + Q = quaternion_optical * Q * quaternion_optical.inverse(); + float3 trans{ex.translation[0], ex.translation[1], ex.translation[2]}; + if (stream == POSE) + { + Q = Q.inverse(); + publish_static_tf(transform_ts_, trans, Q, _frame_id[stream], _base_frame_id); + } + else + { + publish_static_tf(transform_ts_, trans, Q, _base_frame_id, _frame_id[stream]); + publish_static_tf(transform_ts_, zero_trans, quaternion_optical, _frame_id[stream], _optical_frame_id[stream]); + + // Add align_depth_to if exist: + if (_align_depth && _depth_aligned_frame_id.find(stream) != _depth_aligned_frame_id.end()) + { + publish_static_tf(transform_ts_, trans, Q, _base_frame_id, _depth_aligned_frame_id[stream]); + publish_static_tf(transform_ts_, zero_trans, quaternion_optical, _depth_aligned_frame_id[stream], _optical_frame_id[stream]); + } + } +} + +void T265RealsenseNode::warningDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& status) +{ + status.summary(diagnostic_msgs::DiagnosticStatus::WARN, _T265_fault); +} diff --git a/camera_ws/src/realsense-ros/realsense2_camera/srv/DeviceInfo.srv b/camera_ws/src/realsense-ros/realsense2_camera/srv/DeviceInfo.srv new file mode 100644 index 0000000000000000000000000000000000000000..d41600c6399113c1ee1dca0926cb9ed2101b7ff2 --- /dev/null +++ b/camera_ws/src/realsense-ros/realsense2_camera/srv/DeviceInfo.srv @@ -0,0 +1,7 @@ +--- +string device_name +string serial_number +string firmware_version +string usb_type_descriptor +string firmware_update_id +string sensors diff --git a/camera_ws/src/realsense-ros/realsense2_description/CMakeLists.txt b/camera_ws/src/realsense-ros/realsense2_description/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..3b285b434de9e032f79176ffad5505b8dda7e83b --- /dev/null +++ b/camera_ws/src/realsense-ros/realsense2_description/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 2.8.3) +project(realsense2_description) + +find_package(catkin REQUIRED COMPONENTS + ) + +# RealSense description +catkin_package( + ) + +# Install files +install(DIRECTORY launch meshes rviz urdf + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +# Tests +if (CATKIN_ENABLE_TESTING) + catkin_add_nosetests(tests) +endif() \ No newline at end of file diff --git a/camera_ws/src/realsense-ros/realsense2_description/tests/dual_r430.xacro b/camera_ws/src/realsense-ros/realsense2_description/tests/dual_r430.xacro new file mode 100644 index 0000000000000000000000000000000000000000..463e76149ce6d931039e1a4395d249e545562264 --- /dev/null +++ b/camera_ws/src/realsense-ros/realsense2_description/tests/dual_r430.xacro @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/camera_ws/src/realsense-ros/realsense2_description/tests/one_of_each.xacro b/camera_ws/src/realsense-ros/realsense2_description/tests/one_of_each.xacro new file mode 100644 index 0000000000000000000000000000000000000000..58c1bdf016542110ab3f1cf0c5d11243b3b0aeca --- /dev/null +++ b/camera_ws/src/realsense-ros/realsense2_description/tests/one_of_each.xacro @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/camera_ws/src/realsense-ros/realsense2_description/tests/test_xacro.py b/camera_ws/src/realsense-ros/realsense2_description/tests/test_xacro.py new file mode 100644 index 0000000000000000000000000000000000000000..939fced2c92a15c41d6098ff9457ab13018e1a56 --- /dev/null +++ b/camera_ws/src/realsense-ros/realsense2_description/tests/test_xacro.py @@ -0,0 +1,21 @@ +#!/usr/bin/env python + +import rospkg +import subprocess +import os + +r = rospkg.RosPack() +path = r.get_path('realsense2_description') + + +def run_xacro_in_file(filename): + assert(filename != "") + assert(subprocess.check_output(["xacro", "--inorder", "tests/{}".format(filename)], + cwd=path)) + + +def test_files(): + for _, _, filenames in os.walk(os.path.join(path, "tests")): + for file in filenames: + if file.endswith(".xacro"): + yield run_xacro_in_file, file diff --git a/camera_ws/src/realsense-ros/realsense2_description/urdf/_usb_plug.urdf.xacro b/camera_ws/src/realsense-ros/realsense2_description/urdf/_usb_plug.urdf.xacro new file mode 100644 index 0000000000000000000000000000000000000000..af54aede630df9683ac3f01a380651597be5f988 --- /dev/null +++ b/camera_ws/src/realsense-ros/realsense2_description/urdf/_usb_plug.urdf.xacro @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/camera_ws/src/ros_astra_camera/include/astra_camera/point_cloud_proc/point_cloud_xyz.h b/camera_ws/src/ros_astra_camera/include/astra_camera/point_cloud_proc/point_cloud_xyz.h new file mode 100644 index 0000000000000000000000000000000000000000..017dbfb558e976c77ae363cf13171aff2d10f4f5 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/include/astra_camera/point_cloud_proc/point_cloud_xyz.h @@ -0,0 +1,93 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "depth_conversions.h" +#include "depth_traits.h" + +#include "astra_camera/utils.h" +#include "astra_camera/types.h" + +namespace astra_camera { +using namespace message_filters::sync_policies; +namespace enc = sensor_msgs::image_encodings; +using PointCloud2 = sensor_msgs::PointCloud2; + +class PointCloudXyzNode { + public: + PointCloudXyzNode(ros::NodeHandle& nh, ros::NodeHandle& nh_private); + ~PointCloudXyzNode(); + + private: + void connectCb(); + void disconnectCb(); + + void depthCb(const sensor_msgs::ImageConstPtr& depth_msg, + const sensor_msgs::CameraInfoConstPtr& info_msg); + + bool SavePointCloudXyzCallback(std_srvs::Empty::Request& request, + std_srvs::Empty::Response& response); + + private: + ros::NodeHandle nh_; + ros::NodeHandle nh_private_; + std::shared_ptr it_; + image_transport::CameraSubscriber sub_depth_; + ros::ServiceServer save_point_cloud_srv_; + std::atomic_bool save_cloud_{false}; + int queue_size_ = 5; + + // Publications + std::mutex connect_mutex_; + ros::Publisher pub_point_cloud_; + + image_geometry::PinholeCameraModel model_; +}; +} // namespace astra_camera diff --git a/camera_ws/src/ros_astra_camera/launch/astra_pro.launch b/camera_ws/src/ros_astra_camera/launch/astra_pro.launch new file mode 100644 index 0000000000000000000000000000000000000000..8b90c6d76317cd9e4c34bdc7e0a7be2ac1cf3afe --- /dev/null +++ b/camera_ws/src/ros_astra_camera/launch/astra_pro.launch @@ -0,0 +1,113 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/camera_ws/src/ros_astra_camera/launch/dabai.launch b/camera_ws/src/ros_astra_camera/launch/dabai.launch new file mode 100644 index 0000000000000000000000000000000000000000..bb4c371754dd1ea8ea96ff5cc3bdfc7842b6db61 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/launch/dabai.launch @@ -0,0 +1,116 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/camera_ws/src/ros_astra_camera/launch/dabai_dc1.launch b/camera_ws/src/ros_astra_camera/launch/dabai_dc1.launch new file mode 100644 index 0000000000000000000000000000000000000000..757cbd34becca53a754310361579fcb9b3075bca --- /dev/null +++ b/camera_ws/src/ros_astra_camera/launch/dabai_dc1.launch @@ -0,0 +1,111 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/camera_ws/src/ros_astra_camera/launch/dabai_dcw.launch b/camera_ws/src/ros_astra_camera/launch/dabai_dcw.launch new file mode 100644 index 0000000000000000000000000000000000000000..06da7b62aba99ae93028a1430557591976db2e1b --- /dev/null +++ b/camera_ws/src/ros_astra_camera/launch/dabai_dcw.launch @@ -0,0 +1,116 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/camera_ws/src/ros_astra_camera/launch/deeyea.launch b/camera_ws/src/ros_astra_camera/launch/deeyea.launch new file mode 100644 index 0000000000000000000000000000000000000000..25092770601590fe757a66f1ba7a7bfe5f23ea64 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/launch/deeyea.launch @@ -0,0 +1,113 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/camera_ws/src/ros_astra_camera/launch/embedded_s.launch b/camera_ws/src/ros_astra_camera/launch/embedded_s.launch new file mode 100644 index 0000000000000000000000000000000000000000..e2f9f324738edcd46d9495bd07147bac546a1ab0 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/launch/embedded_s.launch @@ -0,0 +1,113 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/camera_ws/src/ros_astra_camera/launch/embedded_u3.launch b/camera_ws/src/ros_astra_camera/launch/embedded_u3.launch new file mode 100644 index 0000000000000000000000000000000000000000..e2f9f324738edcd46d9495bd07147bac546a1ab0 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/launch/embedded_u3.launch @@ -0,0 +1,113 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/camera_ws/src/ros_astra_camera/launch/gemini.launch b/camera_ws/src/ros_astra_camera/launch/gemini.launch new file mode 100644 index 0000000000000000000000000000000000000000..e692e2e98a4875445a29e8cae4fe410e5db3a128 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/launch/gemini.launch @@ -0,0 +1,113 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/camera_ws/src/ros_astra_camera/launch/gemini_e_lite.launch b/camera_ws/src/ros_astra_camera/launch/gemini_e_lite.launch new file mode 100644 index 0000000000000000000000000000000000000000..b49e2b4e5b72b064dee7dcbe9ce1f548a34f9b02 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/launch/gemini_e_lite.launch @@ -0,0 +1,89 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/camera_ws/src/ros_astra_camera/launch/list_devices.launch b/camera_ws/src/ros_astra_camera/launch/list_devices.launch new file mode 100644 index 0000000000000000000000000000000000000000..ba3e57bdb5ba20dbde553d91551ceaf97e81dd90 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/launch/list_devices.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/camera_ws/src/ros_astra_camera/launch/multi_dabai_dcw.launch b/camera_ws/src/ros_astra_camera/launch/multi_dabai_dcw.launch new file mode 100644 index 0000000000000000000000000000000000000000..8a9070261a018989005b7ec5e9ae40dd336487d7 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/launch/multi_dabai_dcw.launch @@ -0,0 +1,25 @@ + + + + + + + + + + 7 + + + + + + + + + + + + + + + diff --git a/camera_ws/src/ros_astra_camera/launch/multi_dabai_dcw2.launch b/camera_ws/src/ros_astra_camera/launch/multi_dabai_dcw2.launch new file mode 100644 index 0000000000000000000000000000000000000000..8e97dae4246d71b83fbfa703b0be25a9085e2c07 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/launch/multi_dabai_dcw2.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/camera_ws/src/ros_astra_camera/launch/multi_deeyea.launch b/camera_ws/src/ros_astra_camera/launch/multi_deeyea.launch new file mode 100644 index 0000000000000000000000000000000000000000..0a411a629ae7a03bf46507b801952c3a42cc9e8d --- /dev/null +++ b/camera_ws/src/ros_astra_camera/launch/multi_deeyea.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/camera_ws/src/ros_astra_camera/launch/multi_device.launch b/camera_ws/src/ros_astra_camera/launch/multi_device.launch new file mode 100644 index 0000000000000000000000000000000000000000..386b60e0571343981e0802af23df5fdfc5e266c4 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/launch/multi_device.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/camera_ws/src/ros_astra_camera/launch/stereo_s_u3.launch b/camera_ws/src/ros_astra_camera/launch/stereo_s_u3.launch new file mode 100644 index 0000000000000000000000000000000000000000..24cf4f9b435505c2c195f4c8e1a43406386ba16f --- /dev/null +++ b/camera_ws/src/ros_astra_camera/launch/stereo_s_u3.launch @@ -0,0 +1,113 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/camera_ws/src/ros_astra_camera/msg/Metadata.msg b/camera_ws/src/ros_astra_camera/msg/Metadata.msg new file mode 100644 index 0000000000000000000000000000000000000000..0c49959c77ce36a77d19877129580fdec2c7a3b8 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/msg/Metadata.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +string json_data diff --git a/camera_ws/src/ros_astra_camera/scripts/create_udev_rules b/camera_ws/src/ros_astra_camera/scripts/create_udev_rules new file mode 100644 index 0000000000000000000000000000000000000000..aceca5d5c971711d067a45d436b991c0b019cfd9 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/scripts/create_udev_rules @@ -0,0 +1,16 @@ +#!/bin/bash + +echo "" +echo "This script copies a udev rule to /etc to facilitate bringing" +echo "up the astra usb connection as /dev/astra*" +echo "" + +sudo cp `rospack find astra_camera`/56-orbbec-usb.rules /etc/udev/rules.d + + +echo "" +echo "Restarting udev" +echo "" +sudo service udev reload +sudo service udev restart +#sudo udevadm trigger --action=change diff --git a/camera_ws/src/ros_astra_camera/scripts/depth_to_color.py b/camera_ws/src/ros_astra_camera/scripts/depth_to_color.py new file mode 100644 index 0000000000000000000000000000000000000000..85e689f090e1d434413cb0c6a7f59f29c6fe208a --- /dev/null +++ b/camera_ws/src/ros_astra_camera/scripts/depth_to_color.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python3 + +import message_filters +from sensor_msgs.msg import Image +import rospy +import cv2 +from cv_bridge import CvBridge +import numpy as np +from tf2_ros import TransformListener,Buffer +import math +import tf2_ros +from geometry_msgs.msg import PointStamped +from tf2_geometry_msgs import do_transform_point + + +tf_listener = None +tf_buffer = None +def callback( depth_msg : Image): + cv_bridge = CvBridge() + depth_img = cv_bridge.imgmsg_to_cv2(depth_msg, "16UC1") + center_x = depth_img.shape[1] / 2 + center_y = depth_img.shape[0] / 2 + try: + trans = tf_buffer.lookup_transform('camera_depth_frame', 'camera_color_frame', rospy.Time(0)) + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: + rospy.loginfo(e) + return + + # Create a point in the depth camera's coordinate frame + depth_point = PointStamped() + depth_point.header.frame_id = 'camera_depth_frame' + depth_point.header.stamp = rospy.Time.now() + depth_point.point.x = center_x + depth_point.point.y = center_y + depth_point.point.z = depth_img[int(center_y), int(center_x)] + + # Now we transform the point from the depth camera's coordinate frame to the color camera's coordinate frame + try: + color_point = do_transform_point(depth_point, trans) + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: + rospy.loginfo(e) + return + + # The point's position in the color frame is now stored in color_point.point + color_x = color_point.point.x + color_y = color_point.point.y + print("from depth {},{} to color {},{}".format(center_x,center_y,color_x,color_y)) + + + +def main(): + rospy.init_node('test_sync', anonymous=True) + depth_sub = rospy.Subscriber('/camera/depth/image_raw', Image, callback) + global tf_listener, tf_buffer + tf_buffer = Buffer() + tf_listener = TransformListener(tf_buffer) + rospy.spin() + + +if __name__ == '__main__': + main() diff --git a/camera_ws/src/ros_astra_camera/scripts/get_point_cloud_dist.py b/camera_ws/src/ros_astra_camera/scripts/get_point_cloud_dist.py new file mode 100644 index 0000000000000000000000000000000000000000..8c68d1c273250fb3493bf3c28957a7eb7ccdb750 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/scripts/get_point_cloud_dist.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python3 + +from numpy import mat +from sensor_msgs.msg import PointCloud2 +import sensor_msgs.point_cloud2 as pc2 + +import rospy +import math + +def pointCloudCb(data): + for p in pc2.read_points(data, field_names = ("x", "y", "z"), skip_nans=True): + dist = p[0]*p[0] + p[1]*p[1] + p[2] *p[2] + print("dist %s" % math.sqrt(dist)) + + +def main(): + rospy.init_node('get_point_cloud_dist', anonymous=True) + rospy.Subscriber("/camera/depth/points", PointCloud2, pointCloudCb) + rospy.spin() + +if __name__ == '__main__': + main() diff --git a/camera_ws/src/ros_astra_camera/scripts/get_supported_video_modes.py b/camera_ws/src/ros_astra_camera/scripts/get_supported_video_modes.py new file mode 100644 index 0000000000000000000000000000000000000000..d8dce89ecb7b2577424f2bfbaa5e334e57700900 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/scripts/get_supported_video_modes.py @@ -0,0 +1,29 @@ +#!/usr/bin/env python3 + +from astra_camera.srv import GetString + +import rospy +import math +import sys +import json + + +def main(): + rospy.init_node('GetSupportedVideoModes', anonymous=True) + stream = sys.argv[1] + service = "/camera/get_" + stream + "_supported_video_modes" + rospy.wait_for_service(service) + try: + res = rospy.ServiceProxy(service, GetString) + response = res() + data = json.loads(response.data) + for item in data: + print("%s" % item) + except rospy.ServiceException as e: + print("Service call failed: %s"%e) + + rospy.spin() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/camera_ws/src/ros_astra_camera/scripts/sync.py b/camera_ws/src/ros_astra_camera/scripts/sync.py new file mode 100644 index 0000000000000000000000000000000000000000..9986f66b2c56dc2bf709427ed4c259c7825ad73b --- /dev/null +++ b/camera_ws/src/ros_astra_camera/scripts/sync.py @@ -0,0 +1,21 @@ +import message_filters +from sensor_msgs.msg import Image +import rospy + + +def callback(depth_msg, rgb_msg): + print("hello") + + +def main(): + rospy.init_node('test_sync', anonymous=True) + rgb_sub = message_filters.Subscriber('/camera/rgb/image_raw', Image) + depth_sub = message_filters.Subscriber('/camera/depth/image_raw', Image) + + ts = message_filters.ApproximateTimeSynchronizer([rgb_sub, depth_sub], 10, 1) + ts.registerCallback(callback) + rospy.spin() + + +if __name__ == '__main__': + main() diff --git a/camera_ws/src/ros_astra_camera/scripts/test_open_close_stream.py b/camera_ws/src/ros_astra_camera/scripts/test_open_close_stream.py new file mode 100644 index 0000000000000000000000000000000000000000..6fafaaf4cf0c422af1b58f322771573aa9a7e844 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/scripts/test_open_close_stream.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python3 +import imp +import rospy +import time +from sensor_msgs.msg import Image +import sys + +get_msg = False + + +def callback(data): + global get_msg + get_msg = True + + +def main(): + rospy.init_node('test_sub', anonymous=True) + try: + for i in range(100000): + sub = rospy.Subscriber("/camera/depth/image_raw", Image, callback) + global get_msg + while not get_msg: + time.sleep(0.1) + get_msg = False + sub.unregister() + print("%d test case passed" % i) + except KeyboardInterrupt: + sys.exit(0) + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/camera_ws/src/ros_astra_camera/src/list_devices_node.cpp b/camera_ws/src/ros_astra_camera/src/list_devices_node.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6647f752826313fa9a834065d5384080b2b7895e --- /dev/null +++ b/camera_ws/src/ros_astra_camera/src/list_devices_node.cpp @@ -0,0 +1,45 @@ +/**************************************************************************/ +/* */ +/* Copyright (c) 2013-2022 Orbbec 3D Technology, Inc */ +/* */ +/* PROPRIETARY RIGHTS of Orbbec 3D Technology are involved in the */ +/* subject matter of this material. All manufacturing, reproduction, use, */ +/* and sales rights pertaining to this subject matter are governed by the */ +/* license agreement. The recipient of this software implicitly accepts */ +/* the terms of the license. */ +/* */ +/**************************************************************************/ +#include + +#include "astra_camera/ob_context.h" + +void DeviceConnectedCallback(const openni::DeviceInfo* device_info) { + std::cout << "Device connected: " << device_info->getName() << std::endl; + auto device = std::make_shared(); + auto uri = device_info->getUri(); + std::cout << "URI: " << uri << std::endl; + device->open(uri); + char serial_number[64]; + int data_size = sizeof(serial_number); + device->getProperty(openni::OBEXTENSION_ID_SERIALNUMBER, serial_number, &data_size); + std::cout << "Serial number: " << serial_number << std::endl; + device->close(); +} + +int main() { + openni::OpenNI::initialize(); + auto disconnected_cb = [](const openni::DeviceInfo* device_info) + { + std::cout << "device " << device_info->getUri() << " disconnected" << std::endl; + }; + + auto context = std::make_unique(disconnected_cb); + auto device_list = context->queryDeviceList(); + + for (auto& device_info : device_list) + { + DeviceConnectedCallback(&device_info); + } + openni::OpenNI::shutdown(); + return 0; +} diff --git a/camera_ws/src/ros_astra_camera/src/main.cpp b/camera_ws/src/ros_astra_camera/src/main.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6cc4b6f407597cf1075827adac1690b500748629 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/src/main.cpp @@ -0,0 +1,27 @@ +/**************************************************************************/ +/* */ +/* Copyright (c) 2013-2022 Orbbec 3D Technology, Inc */ +/* */ +/* PROPRIETARY RIGHTS of Orbbec 3D Technology are involved in the */ +/* subject matter of this material. All manufacturing, reproduction, use, */ +/* and sales rights pertaining to this subject matter are governed by the */ +/* license agreement. The recipient of this software implicitly accepts */ +/* the terms of the license. */ +/* */ +/**************************************************************************/ + +#include + +#include "astra_camera/ob_camera_node_factory.h" +using namespace astra_camera; +int main(int argc, char** argv) { + ROS_INFO_STREAM("Starting camera node..."); + ros::init(argc, argv, "astra_camera_node"); + ros::NodeHandle nh; + ros::NodeHandle nh_private("~"); + ROS_INFO_STREAM("Creating camera node..."); + OBCameraNodeFactory node_factory(nh, nh_private); + ROS_INFO_STREAM("Creating camera node done..."); + ros::spin(); + return 0; +} diff --git a/camera_ws/src/ros_astra_camera/src/point_cloud_proc/point_cloud_xyz.cpp b/camera_ws/src/ros_astra_camera/src/point_cloud_proc/point_cloud_xyz.cpp new file mode 100644 index 0000000000000000000000000000000000000000..98c69bb843e63dd1e8d8935cd04895808ca5d089 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/src/point_cloud_proc/point_cloud_xyz.cpp @@ -0,0 +1,117 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +#include "astra_camera/point_cloud_proc/point_cloud_xyz.h" + +namespace astra_camera { +PointCloudXyzNode::PointCloudXyzNode(ros::NodeHandle &nh, ros::NodeHandle &nh_private) + : nh_(nh), nh_private_(nh_private) { + nh_private_.param("queue_size", queue_size_, 5); + ros::SubscriberStatusCallback connect_cb = std::bind(&PointCloudXyzNode::connectCb, this); + ros::SubscriberStatusCallback disconnect_cb = std::bind(&PointCloudXyzNode::disconnectCb, this); + it_.reset(new image_transport::ImageTransport(nh)); + + std::lock_guard lock_(connect_mutex_); + pub_point_cloud_ = nh_.advertise("depth/points", queue_size_, + connect_cb, disconnect_cb); + save_point_cloud_srv_ = nh_.advertiseService( + "save_point_cloud_xyz", + [this](auto &&req, auto &&res) { return this->SavePointCloudXyzCallback(req, res); }); +} + +PointCloudXyzNode::~PointCloudXyzNode() = default; + +void PointCloudXyzNode::connectCb() { + std::lock_guard lock(connect_mutex_); + if (!sub_depth_) { + image_transport::TransportHints hints("raw", ros::TransportHints(), nh_private_); + sub_depth_ = it_->subscribeCamera("depth/image_raw", queue_size_, &PointCloudXyzNode::depthCb, + this, hints); + } +} + +void PointCloudXyzNode::disconnectCb() { + if (pub_point_cloud_.getNumSubscribers() == 0) { + sub_depth_.shutdown(); + } +} + +void PointCloudXyzNode::depthCb(const sensor_msgs::ImageConstPtr &depth_msg, + const sensor_msgs::CameraInfoConstPtr &info_msg) { + PointCloud2::Ptr cloud_msg(new PointCloud2); + cloud_msg->header = depth_msg->header; + cloud_msg->height = depth_msg->height; + cloud_msg->width = depth_msg->width; + cloud_msg->is_dense = false; + cloud_msg->is_bigendian = false; + sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg); + pcd_modifier.setPointCloud2FieldsByString(1, "xyz"); + + // Update camera model + model_.fromCameraInfo(info_msg); + + if (depth_msg->encoding == enc::TYPE_16UC1 || depth_msg->encoding == enc::MONO16) { + convert(depth_msg, cloud_msg, model_); + } else if (depth_msg->encoding == enc::TYPE_32FC1) { + convert(depth_msg, cloud_msg, model_); + } else { + ROS_WARN_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str()); + return; + } + if (save_cloud_) { + save_cloud_ = false; + auto now = std::time(nullptr); + std::stringstream ss; + ss << std::put_time(std::localtime(&now), "%Y%m%d_%H%M%S"); + auto current_path = boost::filesystem::current_path().string(); + std::string filename = current_path + "/point_cloud/points_xyz_" + ss.str() + ".ply"; + if (!boost::filesystem::exists(current_path + "/point_cloud")) { + boost::filesystem::create_directory(current_path + "/point_cloud"); + } + ROS_INFO_STREAM("Saving point cloud to " << filename); + savePointToPly(cloud_msg, filename); + } + pub_point_cloud_.publish(cloud_msg); +} + +bool PointCloudXyzNode::SavePointCloudXyzCallback(std_srvs::Empty::Request &request, + std_srvs::Empty::Response &response) { + (void)request; + (void)response; + ROS_INFO("SavePointCloudXyzCallback"); + save_cloud_ = true; + return true; +} + +} // namespace astra_camera diff --git a/camera_ws/src/ros_astra_camera/src/ros_service.cpp b/camera_ws/src/ros_astra_camera/src/ros_service.cpp new file mode 100644 index 0000000000000000000000000000000000000000..df100a5ec33602b4bf805f2630340df1319baa7a --- /dev/null +++ b/camera_ws/src/ros_astra_camera/src/ros_service.cpp @@ -0,0 +1,708 @@ +/**************************************************************************/ +/* */ +/* Copyright (c) 2013-2022 Orbbec 3D Technology, Inc */ +/* */ +/* PROPRIETARY RIGHTS of Orbbec 3D Technology are involved in the */ +/* subject matter of this material. All manufacturing, reproduction, use, */ +/* and sales rights pertaining to this subject matter are governed by the */ +/* license agreement. The recipient of this software implicitly accepts */ +/* the terms of the license. */ +/* */ +/**************************************************************************/ +#include "astra_camera/ob_camera_node.h" + +namespace astra_camera { + +void OBCameraNode::setupCameraCtrlServices() { + for (const auto& stream_index : IMAGE_STREAMS) { + if (!enable_[stream_index] || !device_->hasSensor(stream_index.first)) { + //ROS_INFO_STREAM("Stream " << stream_name_[stream_index] << " is disabled"); + continue; + } + auto stream_name = stream_name_[stream_index]; + std::string service_name = "get_" + stream_name + "_exposure"; + get_exposure_srv_[stream_index] = nh_.advertiseService( + service_name, [this, stream_index = stream_index](auto&& request, auto&& response) { + response.success = this->getExposureCallback(request, response, stream_index); + return response.success; + }); + service_name = "set_" + stream_name + "_exposure"; + set_exposure_srv_[stream_index] = nh_.advertiseService( + service_name, [this, stream_index = stream_index](auto&& request, auto&& response) { + response.success = this->setExposureCallback(request, response, stream_index); + return response.success; + }); + service_name = "get_" + stream_name + "_gain"; + get_gain_srv_[stream_index] = nh_.advertiseService( + service_name, [this, stream_index = stream_index](auto&& request, auto&& response) { + response.success = this->getGainCallback(request, response, stream_index); + return response.success; + }); + service_name = "set_" + stream_name + "_gain"; + set_gain_srv_[stream_index] = nh_.advertiseService( + service_name, [this, stream_index = stream_index](auto&& request, auto&& response) { + response.success = this->setGainCallback(request, response, stream_index); + return response.success; + }); + service_name = "set_" + stream_name + "_mirror"; + set_mirror_srv_[stream_index] = + nh_.advertiseService( + service_name, [this, stream_index = stream_index](auto&& request, auto&& response) { + response.success = this->setMirrorCallback(request, response, stream_index); + return response.success; + }); + service_name = "set_" + stream_name + "_auto_exposure"; + set_auto_exposure_srv_[stream_index] = + nh_.advertiseService( + service_name, [this, stream_index = stream_index](auto&& request, auto&& response) { + response.success = this->setAutoExposureCallback(request, response, stream_index); + return response.success; + }); + service_name = "toggle_" + stream_name; + toggle_sensor_srv_[stream_index] = + nh_.advertiseService( + service_name, [this, stream_index = stream_index](auto&& request, auto&& response) { + response.success = this->toggleSensorCallback(request, response, stream_index); + return response.success; + }); + service_name = "get_" + stream_name + "_supported_video_modes"; + get_supported_video_modes_srv_[stream_index] = + nh_.advertiseService( + service_name, [this, stream_index = stream_index](auto&& request, auto&& response) { + response.success = + this->getSupportedVideoModesCallback(request, response, stream_index); + return response.success; + }); + } + get_white_balance_srv_ = nh_.advertiseService( + "get_auto_white_balance", [this](auto&& request, auto&& response) { + response.success = this->getAutoWhiteBalanceEnabledCallback(request, response, COLOR); + return response.success; + }); + set_white_balance_srv_ = nh_.advertiseService( + "set_auto_white_balance", [this](auto&& request, auto&& response) { + response.success = this->setAutoWhiteBalanceEnabledCallback(request, response); + return response.success; + }); + set_fan_enable_srv_ = nh_.advertiseService( + "set_fan", [this](auto&& request, auto&& response) { + response.success = this->setFanEnableCallback(request, response); + return response.success; + }); + set_laser_enable_srv_ = nh_.advertiseService( + "set_laser", [this](auto&& request, auto&& response) { + response.success = this->setLaserEnableCallback(request, response); + return response.success; + }); + //Laser is read only. + // get_laser_status_srv_ = nh_.advertiseService( + // "get_laser_status", [this](auto&& request, auto&& response) { + // response.success = this->getLaserStatusCallback(request, response); + // return response.success; + // }); + set_ldp_enable_srv_ = nh_.advertiseService( + "set_ldp", [this](auto&& request, auto&& response) { + response.success = this->setLdpEnableCallback(request, response); + return response.success; + }); + + get_ldp_status_srv_ = nh_.advertiseService( + "get_ldp_status", [this](auto&& request, auto&& response) { + response.success = this->getLdpStatusCallback(request, response); + return response.success; + }); + get_ir_temperature_srv_ = nh_.advertiseService( + "get_ir_temperature", [this](auto&& request, auto&& response) { + response.success = this->getIRTemperatureCallback(request, response); + return response.success; + }); + + get_device_srv_ = nh_.advertiseService( + "get_device_info", [this](auto&& request, auto&& response) { + response.success = this->getDeviceInfoCallback(request, response); + return response.success; + }); + get_sdk_version_srv_ = nh_.advertiseService( + "get_version", [this](auto&& request, auto&& response) { + response.success = this->getSDKVersionCallback(request, response); + return response.success; + }); + get_camera_info_srv_ = nh_.advertiseService( + "get_camera_info", [this](auto&& request, auto&& response) { + response.success = this->getCameraInfoCallback(request, response); + return response.success; + }); + switch_ir_camera_srv_ = nh_.advertiseService( + "switch_ir_camera", [this](auto&& request, auto&& response) { + response.success = this->switchIRCameraCallback(request, response); + return response.success; + }); + get_camera_params_srv_ = nh_.advertiseService( + "get_camera_params", [this](auto&& request, auto&& response) { + response.success = this->getCameraParamsCallback(request, response); + return response.success; + }); + get_device_type_srv_ = nh_.advertiseService( + "get_device_type", [this](auto&& request, auto&& response) { + response.success = this->getDeviceTypeCallback(request, response); + return response.success; + }); + get_serial_srv_ = nh_.advertiseService( + "get_serial", [this](auto&& request, auto&& response) { + response.success = this->getSerialNumberCallback(request, response); + return response.success; + }); + save_images_srv_ = nh_.advertiseService( + "save_images", [this](auto&& request, auto&& response) { + return this->saveImagesCallback(request, response); + }); + reset_ir_exposure_srv_ = nh_.advertiseService( + "reset_ir_exposure", [this](auto&& request, auto&& response) { + return this->resetIRExposureCallback(request, response); + }); + reset_ir_gain_srv_ = nh_.advertiseService( + "reset_ir_gain", [this](auto&& request, auto&& response) { + return this->resetIRGainCallback(request, response); + }); + set_ir_flood_srv_ = nh_.advertiseService( + "set_ir_flood", [this](auto&& request, auto&& response) { + response.success = this->setIRFloodCallback(request, response); + return response.success; + }); +} + +bool OBCameraNode::setMirrorCallback(std_srvs::SetBoolRequest& request, + std_srvs::SetBoolResponse& response, + const stream_index_pair& stream_index) { + (void)response; + if (!stream_started_[stream_index] || !device_->hasSensor(stream_index.first)) { + std::stringstream ss; + ss << "Stream " << stream_name_[stream_index] << " is not started or does not have a sensor"; + response.message = ss.str(); + ROS_ERROR_STREAM(response.message); + return false; + } + auto stream = streams_.at(stream_index); + stream->setMirroringEnabled(request.data); + return true; +} + +bool OBCameraNode::getExposureCallback(GetInt32Request& request, GetInt32Response& response, + const stream_index_pair& stream_index) { + (void)request; + if (!stream_started_[stream_index] || !device_->hasSensor(stream_index.first)) { + std::stringstream ss; + ss << "Stream " << stream_name_[stream_index] << " is not started or does not have a sensor"; + response.message = ss.str(); + ROS_ERROR_STREAM(response.message); + return false; + } + if (stream_index == COLOR) { + auto stream = streams_.at(stream_index); + auto camera_settings = stream->getCameraSettings(); + if (camera_settings == nullptr) { + response.data = 0; + response.success = false; + response.message = stream_name_[stream_index] + " Camera settings not available"; + return false; + } + response.data = camera_settings->getExposure(); + } else if (stream_index == INFRA1 || stream_index == INFRA2 || stream_index == DEPTH) { + response.data = getIRExposure(); + return true; + } else { + response.message = "Stream not supported get exposure"; + return false; + } + return true; +} + +bool OBCameraNode::setExposureCallback(SetInt32Request& request, SetInt32Response& response, + const stream_index_pair& stream_index) { + if (!stream_started_[stream_index] || !device_->hasSensor(stream_index.first)) { + std::stringstream ss; + ss << "Stream " << stream_name_[stream_index] << " is not started or does not have a sensor"; + response.message = ss.str(); + ROS_ERROR_STREAM(response.message); + return false; + } + if (stream_index == COLOR) { + auto stream = streams_.at(stream_index); + auto camera_settings = stream->getCameraSettings(); + if (camera_settings == nullptr) { + response.success = false; + response.message = stream_name_[stream_index] + " Camera settings not available"; + return false; + } + auto rc = camera_settings->setExposure(request.data); + std::stringstream ss; + if (rc != openni::STATUS_OK) { + ss << "Couldn't set color exposure: " << openni::OpenNI::getExtendedError(); + response.message = ss.str(); + ROS_ERROR_STREAM(response.message); + return false; + } else { + return true; + } + } else if (stream_index == INFRA1 || stream_index == INFRA2 || stream_index == DEPTH) { + auto data = static_cast(request.data); + setIRExposure(data); + return true; + } else { + response.message = "stream not support get gain"; + return false; + } +} + +bool OBCameraNode::getGainCallback(GetInt32Request& request, GetInt32Response& response, + const stream_index_pair& stream_index) { + (void)request; + if (!stream_started_[stream_index] || !device_->hasSensor(stream_index.first)) { + std::stringstream ss; + ss << "Stream " << stream_name_[stream_index] << " is not started or does not have a sensor"; + response.message = ss.str(); + ROS_ERROR_STREAM(response.message); + return false; + } + if (stream_index == COLOR) { + auto stream = streams_.at(stream_index); + auto camera_settings = stream->getCameraSettings(); + if (camera_settings == nullptr) { + response.success = false; + response.message = stream_name_[stream_index] + " Camera settings not available"; + return false; + } + response.data = camera_settings->getGain(); + } else if (stream_index == INFRA1 || stream_index == INFRA2 || stream_index == DEPTH) { + response.data = getIRGain(); + } else { + response.message = "stream not support get gain"; + return false; + } + return true; +} + +bool OBCameraNode::setGainCallback(SetInt32Request& request, SetInt32Response& response, + const stream_index_pair& stream_index) { + (void)response; + if (!stream_started_[stream_index] || !device_->hasSensor(stream_index.first)) { + std::stringstream ss; + ss << "Stream " << stream_name_[stream_index] << " is not started or does not have a sensor"; + response.message = ss.str(); + ROS_ERROR_STREAM(response.message); + return false; + } + if (stream_index == COLOR) { + auto stream = streams_.at(stream_index); + auto camera_settings = stream->getCameraSettings(); + if (camera_settings == nullptr) { + response.success = false; + response.message = stream_name_[stream_index] + " Camera settings not available"; + return false; + } + camera_settings->setGain(request.data); + } else if (stream_index == INFRA1 || stream_index == INFRA2 || stream_index == DEPTH) { + setIRGain(request.data); + } + return true; +} + +bool OBCameraNode::getIRTemperatureCallback(GetDoubleRequest& request, + GetDoubleResponse& response) { + (void)request; + double data = 0; + auto ret = device_->getProperty(XN_MODULE_PROPERTY_RT_IR_TEMP, &data); + if (ret != openni::STATUS_OK) { + response.success = false; + response.message = "Failed to get IR temperature"; + return false; + } + response.data = data; + return true; +} + +void OBCameraNode::setIRAutoExposure(bool status) { + std::lock_guard lock(device_lock_); + device_->setProperty(XN_MODULE_PROPERTY_AE, (uint64_t)status); +} + +int OBCameraNode::getIRExposure() { + int data = 0; + int data_size = 4; + std::lock_guard lock(device_lock_); + device_->getProperty(openni::OBEXTENSION_ID_IR_EXP, (uint32_t*)&data, &data_size); + return data; +} + +void OBCameraNode::setIRExposure(uint32_t data) { + std::lock_guard lock(device_lock_); + device_->setProperty(openni::OBEXTENSION_ID_IR_EXP, data); +} + +int OBCameraNode::getIRGain() { + int data = 0; + int data_size = 4; + std::lock_guard lock(device_lock_); + device_->getProperty(openni::OBEXTENSION_ID_IR_GAIN, (uint8_t*)&data, &data_size); + return data; +} + +void OBCameraNode::setIRGain(int data) { + int data_size = 4; + std::lock_guard lock(device_lock_); + device_->setProperty(openni::OBEXTENSION_ID_IR_GAIN, (uint8_t*)&data, data_size); +} + +std::string OBCameraNode::getSerialNumber() { + char serial_number_str[128] = {0}; + int data_size = sizeof(serial_number_str); + std::lock_guard lock(device_lock_); + device_->getProperty(openni::OBEXTENSION_ID_SERIALNUMBER, (uint8_t*)&serial_number_str, + &data_size); + return serial_number_str; +} + +bool OBCameraNode::getAutoWhiteBalanceEnabledCallback(GetInt32Request& request, + GetInt32Response& response, + const stream_index_pair& stream_index) { + (void)request; + if (!stream_started_[stream_index] || !device_->hasSensor(stream_index.first)) { + std::stringstream ss; + ss << "Stream " << stream_name_[stream_index] << " is not started or does not have a sensor"; + response.message = ss.str(); + ROS_ERROR_STREAM(response.message); + return false; + } + auto stream = streams_.at(stream_index); + auto camera_settings = stream->getCameraSettings(); + if (camera_settings == nullptr) { + response.data = 0; + response.message = stream_name_[stream_index] + " Camera settings not available"; + return false; + } + response.data = camera_settings->getAutoWhiteBalanceEnabled(); + return true; +} + +bool OBCameraNode::setAutoWhiteBalanceEnabledCallback(SetInt32Request& request, + SetInt32Response& response) { + if (!device_->hasSensor(openni::SENSOR_COLOR)) { + response.message = "Color sensor not available"; + ROS_ERROR_STREAM(response.message); + return false; + } + auto stream = streams_.at(COLOR); + auto camera_settings = stream->getCameraSettings(); + if (camera_settings == nullptr) { + response.message = stream_name_[COLOR] + " Camera settings not available"; + ROS_ERROR_STREAM(response.message); + return false; + } + auto data = request.data; + auto rc = camera_settings->setAutoWhiteBalanceEnabled(data); + if (rc != openni::STATUS_OK) { + std::stringstream ss; + ss << " Couldn't set auto white balance: " << openni::OpenNI::getExtendedError(); + response.message = ss.str(); + ROS_ERROR_STREAM(ss.str()); + return false; + } else { + return true; + } +} + +bool OBCameraNode::setAutoExposureCallback(std_srvs::SetBoolRequest& request, + std_srvs::SetBoolResponse& response, + const stream_index_pair& stream_index) { + if (!stream_started_[stream_index] || !device_->hasSensor(stream_index.first)) { + std::stringstream ss; + ss << "Stream " << stream_name_[stream_index] << " is not started or does not have a sensor"; + response.message = ss.str(); + ROS_ERROR_STREAM(response.message); + return false; + } + openni::Status status; + if (stream_index == COLOR) { + auto stream = streams_.at(stream_index); + auto camera_settings = stream->getCameraSettings(); + if (camera_settings == nullptr) { + response.success = false; + response.message = stream_name_[stream_index] + " Camera settings not available"; + ROS_ERROR_STREAM(response.message); + return false; + } + status = camera_settings->setAutoExposureEnabled(request.data); + } else if (stream_index == INFRA1 || stream_index == INFRA2 || stream_index == DEPTH) { + std::lock_guard lock(device_lock_); + status = device_->setProperty(XN_MODULE_PROPERTY_AE, request.data); + } else { + response.message = "Stream not supported set auto exposure"; + return false; + } + + if (status != openni::STATUS_OK) { + std::stringstream ss; + ss << "Couldn't set auto exposure: " << openni::OpenNI::getExtendedError(); + response.message = ss.str(); + ROS_ERROR_STREAM(response.message); + return false; + } else { + return true; + } +} + +bool OBCameraNode::setLaserEnableCallback(std_srvs::SetBoolRequest& request, + std_srvs::SetBoolResponse& response) { + std::lock_guard lock(device_lock_); + device_->setProperty(openni::OBEXTENSION_ID_LASER_EN, request.data); + device_->setProperty(XN_MODULE_PROPERTY_EMITTER_STATE, request.data); + response.success = true; + return true; +} + +bool OBCameraNode::setIRFloodCallback(std_srvs::SetBoolRequest& request, + std_srvs::SetBoolResponse& response) { + (void)response; + std::lock_guard lock(device_lock_); + ROS_INFO_STREAM("Setting IR flood to " << (request.data ? "true" : "false")); + int data = static_cast(request.data); + device_->setProperty(XN_MODULE_PROPERTY_IRFLOOD_STATE, data); + return true; +} + +bool OBCameraNode::setLdpEnableCallback(std_srvs::SetBoolRequest& request, + std_srvs::SetBoolResponse& response) { + (void)response; + //stopStreams(); + std::lock_guard lock(device_lock_); + auto status = device_->setProperty(XN_MODULE_PROPERTY_LDP_ENABLE, request.data); + //startStreams(); + if (status != openni::STATUS_OK) { + std::stringstream ss; + ss << "Couldn't set LDP enable: " << openni::OpenNI::getExtendedError(); + ROS_ERROR_STREAM(ss.str()); + response.message = ss.str(); + return false; + } else { + return true; + } +} + +bool OBCameraNode::setFanEnableCallback(std_srvs::SetBoolRequest& request, + std_srvs::SetBoolResponse& response) { + std::lock_guard lock(device_lock_); + device_->setProperty(XN_MODULE_PROPERTY_FAN_ENABLE, request.data); + response.success = true; + return true; +} + +bool OBCameraNode::getDeviceInfoCallback(GetDeviceInfoRequest& request, + GetDeviceInfoResponse& response) { + (void)request; + std::lock_guard lock(device_lock_); + auto device_info = device_->getDeviceInfo(); + response.info.name = device_info.getName(); + response.info.pid = device_info.getUsbProductId(); + response.info.vid = device_info.getUsbVendorId(); + char serial_number[64]; + int data_size = sizeof(serial_number); + auto rc = device_->getProperty(openni::OBEXTENSION_ID_SERIALNUMBER, serial_number, &data_size); + if (rc == openni::STATUS_OK) { + response.info.serial_number = serial_number; + } else { + response.info.serial_number = ""; + } + return true; +} + +bool OBCameraNode::getCameraInfoCallback(GetCameraInfoRequest& request, + GetCameraInfoResponse& response) { + (void)request; + std::lock_guard lock(device_lock_); + auto camera_info = getColorCameraInfo(); + response.info = camera_info; + return true; +} + +bool OBCameraNode::getSDKVersionCallback(GetStringRequest& request, GetStringResponse& response) { + (void)request; + nlohmann::json data; + data["ros_sdk_version"] = OB_ROS_VERSION_STR; + data["openni_version"] = ONI_VERSION_STRING; + char buffer[128] = {0}; + int data_size = sizeof(buffer); + std::lock_guard lock(device_lock_); + device_->getProperty(XN_MODULE_PROPERTY_SENSOR_PLATFORM_STRING, buffer, &data_size); + data["firmware_version"] = buffer; + response.data = data.dump(2); + return true; +} + +bool OBCameraNode::getDeviceTypeCallback(GetStringRequest& request, GetStringResponse& response) { + (void)request; + char device_type_str[128] = {0}; + int data_size = sizeof(device_type_str); + std::lock_guard lock(device_lock_); + device_->getProperty(openni::OBEXTENSION_ID_DEVICETYPE, (uint8_t*)&device_type_str, &data_size); + response.data = device_type_str; + return true; +} + +bool OBCameraNode::getSerialNumberCallback(GetStringRequest& request, GetStringResponse& response) { + (void)request; + response.data = getSerialNumber(); + return true; +} + +bool OBCameraNode::switchIRCameraCallback(SetStringRequest& request, SetStringResponse& response) { + const int data_size = 4; + int data; + std::lock_guard lock(device_lock_); + if (request.data == "left") { + data = 0; + device_->setProperty(XN_MODULE_PROPERTY_SWITCH_IR, (uint8_t*)&data, data_size); + } else if (request.data == "right") { + data = 1; + device_->setProperty(XN_MODULE_PROPERTY_SWITCH_IR, (uint8_t*)&data, data_size); + } else { + response.message = "Invalid IR camera name"; + ROS_ERROR_STREAM(response.message); + return false; + } + return true; +} + +bool OBCameraNode::getCameraParamsCallback(GetCameraParamsRequest& request, + GetCameraParamsResponse& response) { + (void)request; + auto camera_params = getCameraParams(); + for (int i = 0; i < 9; i++) { + response.r2l_r[i] = camera_params.r2l_r[i]; + if (i < 4) { + response.l_intr_p[i] = camera_params.l_intr_p[i]; + response.r_intr_p[i] = camera_params.r_intr_p[i]; + } + if (i < 3) { + response.r2l_t[i] = camera_params.r2l_t[i]; + } + if (i < 5) { + response.l_k[i] = camera_params.l_k[i]; + response.r_k[i] = camera_params.r_k[i]; + } + } + return true; +} + +bool OBCameraNode::toggleSensorCallback(std_srvs::SetBoolRequest& request, + std_srvs::SetBoolResponse& response, + const stream_index_pair& stream_index) { + if (request.data) { + ROS_INFO_STREAM(stream_name_[stream_index] << " ON"); + } else { + ROS_INFO_STREAM(stream_name_[stream_index] << " OFF"); + } + response.success = toggleSensor(stream_index, request.data, response.message); + return true; +} + +bool OBCameraNode::saveImagesCallback(std_srvs::EmptyRequest& request, + std_srvs::EmptyResponse& response) { + (void)request; + (void)response; + ROS_INFO_STREAM("Saving images"); + if (enable_[INFRA1]) { + save_images_[INFRA1] = true; + } + if (enable_[COLOR]) { + save_images_[COLOR] = true; + } + if (enable_[DEPTH]) { + save_images_[DEPTH] = true; + } + return true; +} + +bool OBCameraNode::getSupportedVideoModesCallback(GetStringRequest& request, + GetStringResponse& response, + const stream_index_pair& stream_index) { + (void)request; + if (!supported_video_modes_.count(stream_index)) { + response.data = ""; + response.message = "No supported video modes"; + return false; + } else { + auto modes = supported_video_modes_[stream_index]; + nlohmann::json data; + for (auto& mode : modes) { + std::stringstream ss; + ss << mode.getResolutionX() << "x" << mode.getResolutionY() << "@" << mode.getFps(); + if (data.empty() || data.back() != ss.str()) { + data.push_back(ss.str()); + } + } + response.data = data.dump(2); + return true; + } +} + +// bool OBCameraNode::getLaserStatusCallback(GetBoolRequest& request, GetBoolResponse& response) { +// (void)request; +// std::lock_guard lock(device_lock_); +// int data = 0; +// int data_size = 4; +// device_->getProperty(XN_MODULE_PROPERTY_EMITTER_STATE, (uint8_t*)&data, &data_size); +// response.data = data; +// return true; +// } + +bool OBCameraNode::getLdpStatusCallback(GetBoolRequest& request, GetBoolResponse& response) { + (void)request; + std::lock_guard lock(device_lock_); + int data = 0; + int data_size = 4; + device_->getProperty(XN_MODULE_PROPERTY_LDP_STATUS, (uint8_t*)&data, &data_size); + response.data = data; + return true; +} + +bool OBCameraNode::resetIRGainCallback(std_srvs::EmptyRequest& request, + std_srvs::EmptyResponse& response) { + (void)request; + (void)response; + ROS_INFO_STREAM("Resetting IR gain"); + setIRGain(init_ir_gain_); + return true; +} + +bool OBCameraNode::resetIRExposureCallback(std_srvs::EmptyRequest& request, + std_srvs::EmptyResponse& response) { + (void)request; + (void)response; + ROS_INFO_STREAM("Resetting IR exposure"); + setIRExposure(init_ir_exposure_); + return true; +} + +bool OBCameraNode::toggleSensor(const stream_index_pair& stream_index, bool enabled, + std::string& msg) { + std::lock_guard lock(device_lock_); + if (!device_->hasSensor(stream_index.first)) { + std::stringstream ss; + + ss << "doesn't have " << stream_name_[stream_index]; + msg = ss.str(); + ROS_WARN_STREAM(msg); + return false; + } + + if (enabled) { + enable_[stream_index] = true; + } else { + enable_[stream_index] = false; + } + stopStreams(); + startStreams(); + return true; +} + +} // namespace astra_camera diff --git a/camera_ws/src/ros_astra_camera/src/utils.cpp b/camera_ws/src/ros_astra_camera/src/utils.cpp new file mode 100644 index 0000000000000000000000000000000000000000..9167043be9236b5514cdd189bb9cec633d3d927e --- /dev/null +++ b/camera_ws/src/ros_astra_camera/src/utils.cpp @@ -0,0 +1,210 @@ +/**************************************************************************/ +/* */ +/* Copyright (c) 2013-2022 Orbbec 3D Technology, Inc */ +/* */ +/* PROPRIETARY RIGHTS of Orbbec 3D Technology are involved in the */ +/* subject matter of this material. All manufacturing, reproduction, use, */ +/* and sales rights pertaining to this subject matter are governed by the */ +/* license agreement. The recipient of this software implicitly accepts */ +/* the terms of the license. */ +/* */ +/**************************************************************************/ +#include "astra_camera/utils.h" + +#include + +#include "sensor_msgs/PointCloud.h" +#include "sensor_msgs/distortion_models.h" +#include "sensor_msgs/point_cloud2_iterator.h" + +namespace astra_camera { + +bool operator==(const openni::VideoMode& lhs, const openni::VideoMode& rhs) { + return lhs.getResolutionY() == rhs.getResolutionY() && + lhs.getResolutionX() == rhs.getResolutionX() && lhs.getFps() == rhs.getFps() && + lhs.getPixelFormat() == rhs.getPixelFormat(); +} + +bool operator!=(const openni::VideoMode& lhs, const openni::VideoMode& rhs) { + return !(lhs == rhs); +} + +std::ostream& operator<<(std::ostream& os, const openni::VideoMode& video_mode) { + os << "Resolution :" << video_mode.getResolutionX() << "x" << video_mode.getResolutionY() << "@" + << video_mode.getFps() << "Hz" << std::endl + << "format " << PixelFormatToString(video_mode.getPixelFormat()); + return os; +} + +tf2::Quaternion rotationMatrixToQuaternion(const std::vector& rotation) { + CHECK_EQ(rotation.size(), 9u); + Eigen::Matrix3f m; + // We need to be careful about the order, as RS2 rotation matrix is + // column-major, while Eigen::Matrix3f expects row-major. + m << rotation[0], rotation[3], rotation[6], rotation[1], rotation[4], rotation[7], rotation[2], + rotation[5], rotation[8]; + Eigen::Quaternionf q(m); + return {q.x(), q.y(), q.z(), q.w()}; +} + +Extrinsics obExtrinsicsToMsg(const std::vector& rotation, + const std::vector& transition, const std::string& frame_id) { + CHECK_EQ(rotation.size(), 9u); + CHECK_EQ(transition.size(), 3u); + Extrinsics msg; + for (int i = 0; i < 9; ++i) { + msg.rotation[i] = rotation[i]; + if (i < 3) { + msg.translation[i] = transition[i]; + } + } + + msg.header.frame_id = frame_id; + return msg; +} + +bool isValidCameraParams(const OBCameraParams& params) { + if (std::isnan(params.l_intr_p[0]) || std::isnan(params.l_intr_p[1]) || + std::isnan(params.l_intr_p[2]) || std::isnan(params.l_intr_p[3])) { + return false; + } + return true; +} + +void cameraParameterPrinter(const std::vector& rotation, + const std::vector& transition) { + CHECK_EQ(rotation.size(), 9u); + CHECK_EQ(transition.size(), 3u); + std::cout << "Rotation: " << std::endl; + for (int i = 0; i < 9; ++i) { + std::cout << rotation[i] << " "; + } + std::cout << std::endl; + std::cout << "Translation: " << std::endl; + for (int i = 0; i < 3; ++i) { + std::cout << transition[i] << " "; + } + std::cout << std::endl; +} + +std::string PixelFormatToString(const openni::PixelFormat& format) { + switch (format) { + case openni::PIXEL_FORMAT_DEPTH_1_MM: + return "PIXEL_FORMAT_DEPTH_1_MM"; + case openni::PIXEL_FORMAT_DEPTH_100_UM: + return "PIXEL_FORMAT_DEPTH_100_UM"; + case openni::PIXEL_FORMAT_SHIFT_9_2: + return "PIXEL_FORMAT_SHIFT_9_2"; + case openni::PIXEL_FORMAT_SHIFT_9_3: + return "PIXEL_FORMAT_SHIFT_9_3"; + case openni::PIXEL_FORMAT_RGB888: + return "PIXEL_FORMAT_RGB888"; + case openni::PIXEL_FORMAT_YUV422: + return "PIXEL_FORMAT_YUV422"; + case openni::PIXEL_FORMAT_GRAY8: + return "PIXEL_FORMAT_GRAY8"; + case openni::PIXEL_FORMAT_GRAY16: + return "PIXEL_FORMAT_GRAY16"; + case openni::PIXEL_FORMAT_JPEG: + return "PIXEL_FORMAT_JPEG"; + case openni::PIXEL_FORMAT_YUYV: + return "PIXEL_FORMAT_YUYV"; + case openni::PIXEL_FORMAT_LOG: + return "PIXEL_FORMAT_LOG"; + } + return "Unknown"; +} +void savePointToPly(sensor_msgs::PointCloud2::Ptr cloud, const std::string& filename) { + sensor_msgs::PointCloud out_point_cloud; + sensor_msgs::convertPointCloud2ToPointCloud(*cloud, out_point_cloud); + size_t point_size = 0; + FILE* fp = fopen(filename.c_str(), "wb+"); + for (auto& point : out_point_cloud.points) { + if (!std::isnan(point.x) && !std::isnan(point.y) && !std::isnan(point.z)) { + point_size++; + } + } + ROS_INFO_STREAM("Point size: " << point_size); + fprintf(fp, "ply\n"); + fprintf(fp, "format ascii 1.0\n"); + fprintf(fp, "element vertex %zu\n", point_size); + fprintf(fp, "property float x\n"); + fprintf(fp, "property float y\n"); + fprintf(fp, "property float z\n"); + fprintf(fp, "end_header\n"); + for (const auto& point : out_point_cloud.points) { + if (!std::isnan(point.x) && !std::isnan(point.y) && !std::isnan(point.z)) { + fprintf(fp, "%.3f %.3f %.3f\n", point.x, point.y, point.z); + } + } + fflush(fp); + fclose(fp); +} + +void saveRGBPointToPly(sensor_msgs::PointCloud2::Ptr cloud, const std::string& filename) { + sensor_msgs::PointCloud2Iterator iter_x(*cloud, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*cloud, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*cloud, "z"); + sensor_msgs::PointCloud2Iterator iter_r(*cloud, "r"); + sensor_msgs::PointCloud2Iterator iter_g(*cloud, "g"); + sensor_msgs::PointCloud2Iterator iter_b(*cloud, "b"); + size_t point_size = cloud->width * cloud->height; + std::vector points; + std::vector> rgb; + for (size_t i = 0; i < point_size; i++) { + if (!std::isnan(*iter_x) && !std::isnan(*iter_y) && !std::isnan(*iter_z)) { + geometry_msgs::Point32 point; + point.x = *iter_x; + point.y = *iter_y; + point.z = *iter_z; + points.push_back(point); + std::vector rgb_point; + rgb_point.push_back(*iter_r); + rgb_point.push_back(*iter_g); + rgb_point.push_back(*iter_b); + rgb.push_back(rgb_point); + } + ++iter_x; + ++iter_y; + ++iter_z; + ++iter_r; + ++iter_g; + ++iter_b; + } + point_size = points.size(); + FILE* fp = fopen(filename.c_str(), "wb+"); + + ROS_INFO_STREAM("Point size: " << point_size); + fprintf(fp, "ply\n"); + fprintf(fp, "format ascii 1.0\n"); + fprintf(fp, "element vertex %zu\n", point_size); + fprintf(fp, "property float x\n"); + fprintf(fp, "property float y\n"); + fprintf(fp, "property float z\n"); + fprintf(fp, "property uchar red\n"); + fprintf(fp, "property uchar green\n"); + fprintf(fp, "property uchar blue\n"); + fprintf(fp, "end_header\n"); + + for (size_t i = 0; i < point_size; ++i) { + fprintf(fp, "%.3f %.3f %.3f %d %d %d\n", points[i].x, points[i].y, points[i].z, rgb[i][0], + rgb[i][1], rgb[i][2]); + } + fflush(fp); + fclose(fp); +} + +MultiDeviceSyncMode getMultiDeviceSyncMode(const std::string& mode) { + if (mode == "none") { + return MultiDeviceSyncMode::None; + } else if (mode == "master" || mode == "main") { + return MultiDeviceSyncMode::Master; + } else if (mode == "slave" || mode == "sub") { + return MultiDeviceSyncMode::Slave; + } else { + return MultiDeviceSyncMode::None; + } +} + + +} // namespace astra_camera diff --git a/camera_ws/src/ros_astra_camera/srv/GetBool.srv b/camera_ws/src/ros_astra_camera/srv/GetBool.srv new file mode 100644 index 0000000000000000000000000000000000000000..ccfce7dee3a602c5a599cc7768ed172ce30bbf21 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/srv/GetBool.srv @@ -0,0 +1,4 @@ +--- +bool data +bool success +string message diff --git a/camera_ws/src/ros_astra_camera/srv/GetCameraInfo.srv b/camera_ws/src/ros_astra_camera/srv/GetCameraInfo.srv new file mode 100644 index 0000000000000000000000000000000000000000..112f2b71da9034fb59042f1bc0073731ef624846 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/srv/GetCameraInfo.srv @@ -0,0 +1,4 @@ +--- +sensor_msgs/CameraInfo info +bool success +string message diff --git a/camera_ws/src/ros_astra_camera/srv/GetCameraParams.srv b/camera_ws/src/ros_astra_camera/srv/GetCameraParams.srv new file mode 100644 index 0000000000000000000000000000000000000000..c98c0ba5b8b31d41c958154436ee355c1354b8f8 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/srv/GetCameraParams.srv @@ -0,0 +1,9 @@ +--- +float32[4] l_intr_p +float32[4] r_intr_p +float32[9] r2l_r +float32[3] r2l_t +float32[5] l_k +float32[5] r_k +bool success +string message diff --git a/camera_ws/src/ros_astra_camera/srv/GetDeviceInfo.srv b/camera_ws/src/ros_astra_camera/srv/GetDeviceInfo.srv new file mode 100644 index 0000000000000000000000000000000000000000..9f3e1d22fc5b58733801727790734fa07de4697d --- /dev/null +++ b/camera_ws/src/ros_astra_camera/srv/GetDeviceInfo.srv @@ -0,0 +1,4 @@ +--- +DeviceInfo info +bool success +string message diff --git a/camera_ws/src/ros_astra_camera/srv/GetDouble.srv b/camera_ws/src/ros_astra_camera/srv/GetDouble.srv new file mode 100644 index 0000000000000000000000000000000000000000..85bab629ef25baddd31bd099c25ac9093d6600d3 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/srv/GetDouble.srv @@ -0,0 +1,4 @@ +--- +float64 data +bool success +string message diff --git a/camera_ws/src/ros_astra_camera/srv/GetInt32.srv b/camera_ws/src/ros_astra_camera/srv/GetInt32.srv new file mode 100644 index 0000000000000000000000000000000000000000..6621dd843ff0067aad9f941002b8786195820be0 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/srv/GetInt32.srv @@ -0,0 +1,4 @@ +--- +int32 data +bool success +string message diff --git a/camera_ws/src/ros_astra_camera/srv/SetInt32.srv b/camera_ws/src/ros_astra_camera/srv/SetInt32.srv new file mode 100644 index 0000000000000000000000000000000000000000..ead7b28a1b99605f53fec95f1eedc07ff8bd151d --- /dev/null +++ b/camera_ws/src/ros_astra_camera/srv/SetInt32.srv @@ -0,0 +1,4 @@ +int32 data +--- +bool success +string message diff --git a/camera_ws/src/ros_astra_camera/srv/SetString.srv b/camera_ws/src/ros_astra_camera/srv/SetString.srv new file mode 100644 index 0000000000000000000000000000000000000000..b0de4d81a5845e227212267f45e195260f9f7642 --- /dev/null +++ b/camera_ws/src/ros_astra_camera/srv/SetString.srv @@ -0,0 +1,4 @@ +string data +--- +bool success +string message diff --git a/camera_ws/src/ros_astra_camera/test/.gitkeep b/camera_ws/src/ros_astra_camera/test/.gitkeep new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391