metablock.hpp

Go to the documentation of this file.

/*
 * Copyright 2016 SoftBank Robotics Europe
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file 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.
*/

#ifndef METABLOCK_H
#define METABLOCK_H

// ROS
#include <ros/ros.h>

#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_listener.h>

#include <geometric_shapes/solid_primitive_dims.h>

#include <moveit_msgs/CollisionObject.h>

#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <object_recognition_msgs/RecognizedObjectArray.h>

typedef shape_msgs::SolidPrimitive sprimitive;
typedef moveit_msgs::CollisionObject mcollobj;
typedef moveit::planning_interface::PlanningSceneInterface mscene;

namespace moveit_simple_actions
{

class MetaBlock
{
public:
  MetaBlock(const std::string name,
            const geometry_msgs::Pose pose,
            const uint &shapeType,
            const float &size_x,
            const float &size_y,
            const float &size_z,
            ros::Time timestamp=ros::Time::now(),
            std::string base_frame="base_link");

  MetaBlock(const std::string name,
            const geometry_msgs::Pose pose,
            const shape_msgs::Mesh mesh,
            const object_recognition_msgs::ObjectType type,
            ros::Time timestamp=ros::Time::now(),
            std::string base_frame="odom");

  void updatePose(const geometry_msgs::Pose &pose);

  void updatePoseVis(const geometry_msgs::Pose &pose);

  void updatePose(mscene *current_scene,
                  const geometry_msgs::Pose &pose);

  void removeBlock(mscene *current_scene);

  void publishBlock(mscene *current_scene);

  tf::Stamped<tf::Pose> getTransform(tf::TransformListener *listener,
                                     const std::string &frame);

  geometry_msgs::PoseStamped getTransformed(tf::TransformListener *listener,
                                            const std::string &frame);

  std::string name_;

  geometry_msgs::Pose pose_;

  geometry_msgs::Pose goal_pose_;

  moveit_msgs::CollisionObject collObj_;

  double size_x_;

  double size_y_;

  double size_z_;

  ros::Time timestamp_;

  std::string base_frame_;

  object_recognition_msgs::ObjectType type_;

protected:
  shape_msgs::SolidPrimitive shape_;

  shape_msgs::Mesh mesh_;
};
}

#endif // METABLOCK_H