Skip to content

Commit

Permalink
pr fixup
Browse files Browse the repository at this point in the history
  • Loading branch information
Mike Lautman committed Aug 18, 2019
1 parent 3885dc0 commit b274866
Show file tree
Hide file tree
Showing 2 changed files with 72 additions and 50 deletions.
64 changes: 45 additions & 19 deletions include/moveit_visual_tools/moveit_visual_tools.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -323,56 +323,82 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);

/**
* \brief Create/Publish a MoveIt Collision block at the given pose
* \param pose - location of center of block
* \brief Create a MoveIt Collision block at the given pose
* \param block_pose - location of center of block
* \param name - semantic name of MoveIt collision object
* \param size - height=width=depth=size
* \param color to display the collision object with
* \param size - height=width=depth=block_size
* \return true on sucess
**/
moveit_msgs::CollisionObject createCollisionBlock(const geometry_msgs::Pose& block_pose, const std::string& name,
double block_size);
moveit_msgs::CollisionObject createBlockCollisionObjectMsg(const geometry_msgs::Pose& block_pose,
const std::string& name, double block_size = 0.1);

/**
* \brief Publish a MoveIt Collision block at the given pose
* \param block_pose - location of center of block
* \param name - semantic name of MoveIt collision object
* \param size - height=width=depth=block_size
* \param color to display the collision object with
* \return true on sucess
**/
bool publishCollisionBlock(const geometry_msgs::Pose& block_pose, const std::string& block_name = "block",
double block_size = 0.1,
const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);

/**
* \brief Create/Publish a MoveIt collision rectangular cuboid at the given pose
* \brief Create a cuboid MoveIt collision object msg
* \param point1 - top left of rectangle
* \param point2 - bottom right of rectangle
* \param name - semantic name of MoveIt collision object
* \return moveit_msgs::CollisionObject
**/
moveit_msgs::CollisionObject createCuboidCollisionObjectMsg(const Eigen::Vector3d& point1,
const Eigen::Vector3d& point2, const std::string& name);
moveit_msgs::CollisionObject createCuboidCollisionObjectMsg(const geometry_msgs::Point& point1,
const geometry_msgs::Point& point2,
const std::string& name);

/**
* \brief Publish a cuboid MoveIt collision object between two points
* \param point1 - top left of rectangle
* \param point2 - bottom right of rectangle
* \param name - semantic name of MoveIt collision object
* \param color to display the collision object with
* \return true on sucess
**/
moveit_msgs::CollisionObject createCollisionObject(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2,
const std::string& name);
moveit_msgs::CollisionObject createCollisionObject(const geometry_msgs::Point& point1,
const geometry_msgs::Point& point2, const std::string& name);
bool publishCollisionCuboid(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, const std::string& name,
const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
bool publishCollisionCuboid(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2,
const std::string& name,
const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);

/**
* \brief Create/Publish a MoveIt collision rectangular cuboid at the given pose
* \param pose - position of the centroid of the cube
* \brief Create a cuboid MoveIt collision object msg
* \param block_pose - position of the centroid of the cube
* \param width - width of the object in its local frame
* \param depth - depth of the object in its local frame
* \param height - height of the object in its local frame
* \param name - semantic name of MoveIt collision object
* \return moveit_msgs::CollisionObject
**/
moveit_msgs::CollisionObject createCuboidCollisionObjectMsg(const Eigen::Isometry3d& block_pose, double width,
double depth, double height, const std::string& name);
moveit_msgs::CollisionObject createCuboidCollisionObjectMsg(const geometry_msgs::Pose& block_pose, double width,
double depth, double height, const std::string& name);

/**
* \brief Publish a cuboid MoveIt collision object at the given pose
* \param block_pose - position of the centroid of the cube
* \param width - width of the object in its local frame
* \param depth - depth of the object in its local frame
* \param height - height of the object in its local frame
* \param name - semantic name of MoveIt collision object
* \param color to display the collision object with
* \return true on sucess
**/
moveit_msgs::CollisionObject createCollisionObject(const Eigen::Isometry3d& pose, double width, double depth,
double height, const std::string& name);
moveit_msgs::CollisionObject createCollisionObject(const geometry_msgs::Pose& pose, double width, double depth,
double height, const std::string& name);
bool publishCollisionCuboid(const Eigen::Isometry3d& pose, double width, double depth, double height,
bool publishCollisionCuboid(const Eigen::Isometry3d& block_pose, double width, double depth, double height,
const std::string& name, const rviz_visual_tools::colors& color);

bool publishCollisionCuboid(const geometry_msgs::Pose& pose, double width, double depth, double height,
bool publishCollisionCuboid(const geometry_msgs::Pose& block_pose, double width, double depth, double height,
const std::string& name, const rviz_visual_tools::colors& color);

/**
Expand Down
58 changes: 27 additions & 31 deletions src/moveit_visual_tools.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -618,8 +618,9 @@ bool MoveItVisualTools::attachCO(const std::string& name, const std::string& ee_
return processAttachedCollisionObjectMsg(aco);
}

moveit_msgs::CollisionObject MoveItVisualTools::createCollisionBlock(const geometry_msgs::Pose& block_pose,
const std::string& name, double block_size)
moveit_msgs::CollisionObject MoveItVisualTools::createBlockCollisionObjectMsg(const geometry_msgs::Pose& block_pose,
const std::string& name,
double block_size)
{
moveit_msgs::CollisionObject collision_obj;
collision_obj.header.stamp = ros::Time::now();
Expand All @@ -643,12 +644,12 @@ moveit_msgs::CollisionObject MoveItVisualTools::createCollisionBlock(const geome
bool MoveItVisualTools::publishCollisionBlock(const geometry_msgs::Pose& block_pose, const std::string& name,
double block_size, const rviz_visual_tools::colors& color)
{
return processCollisionObjectMsg(createCollisionBlock(block_pose, name, block_size), color);
return processCollisionObjectMsg(createBlockCollisionObjectMsg(block_pose, name, block_size), color);
}

moveit_msgs::CollisionObject MoveItVisualTools::createCollisionObject(const geometry_msgs::Point& point1,
const geometry_msgs::Point& point2,
const std::string& name)
moveit_msgs::CollisionObject MoveItVisualTools::createCuboidCollisionObjectMsg(const geometry_msgs::Point& point1,
const geometry_msgs::Point& point2,
const std::string& name)
{
moveit_msgs::CollisionObject collision_obj;
collision_obj.header.stamp = ros::Time::now();
Expand Down Expand Up @@ -682,11 +683,11 @@ moveit_msgs::CollisionObject MoveItVisualTools::createCollisionObject(const geom
return collision_obj;
}

moveit_msgs::CollisionObject MoveItVisualTools::createCollisionObject(const Eigen::Vector3d& point1,
const Eigen::Vector3d& point2,
const std::string& name)
moveit_msgs::CollisionObject MoveItVisualTools::createCuboidCollisionObjectMsg(const Eigen::Vector3d& point1,
const Eigen::Vector3d& point2,
const std::string& name)
{
return createCollisionObject(convertPoint(point1), convertPoint(point2), name);
return createCuboidCollisionObjectMsg(convertPoint(point1), convertPoint(point2), name);
}

bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2,
Expand All @@ -698,23 +699,22 @@ bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Vector3d& point1, co
bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2,
const std::string& name, const rviz_visual_tools::colors& color)
{
// ROS_INFO_STREAM_NAMED(LOGNAME,"CollisionObject: \n " << collision_obj);
return processCollisionObjectMsg(createCollisionObject(point1, point2, name), color);
return processCollisionObjectMsg(createCuboidCollisionObjectMsg(point1, point2, name), color);
}

moveit_msgs::CollisionObject MoveItVisualTools::createCollisionObject(const geometry_msgs::Pose& pose, double width,
double depth, double height,
const std::string& name)
moveit_msgs::CollisionObject MoveItVisualTools::createCuboidCollisionObjectMsg(const geometry_msgs::Pose& block_pose,
double width, double depth,
double height, const std::string& name)
{
moveit_msgs::CollisionObject collision_obj;
collision_obj.header.stamp = ros::Time::now();
collision_obj.header.frame_id = base_frame_;
collision_obj.id = name;
collision_obj.operation = moveit_msgs::CollisionObject::ADD;

// Calculate center pose
// Calculate center block_pose
collision_obj.primitive_poses.resize(1);
collision_obj.primitive_poses[0] = pose;
collision_obj.primitive_poses[0] = block_pose;

// Calculate scale
collision_obj.primitives.resize(1);
Expand All @@ -736,29 +736,25 @@ moveit_msgs::CollisionObject MoveItVisualTools::createCollisionObject(const geom
return collision_obj;
}

moveit_msgs::CollisionObject MoveItVisualTools::createCollisionObject(const Eigen::Isometry3d& pose, double width,
double depth, double height,
const std::string& name)
moveit_msgs::CollisionObject MoveItVisualTools::createCuboidCollisionObjectMsg(const Eigen::Isometry3d& block_pose,
double width, double depth,
double height, const std::string& name)
{
geometry_msgs::Pose pose_msg = tf2::toMsg(pose);
return createCollisionObject(pose_msg, width, depth, height, name, color);
return createCuboidCollisionObjectMsg(convertPose(block_pose), width, depth, height, name);
}

bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Isometry3d& pose, double width, double depth, double height,
const std::string& name, const rviz_visual_tools::colors& color)
bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Isometry3d& block_pose, double width, double depth,
double height, const std::string& name,
const rviz_visual_tools::colors& color)
{
geometry_msgs::Pose pose_msg = tf2::toMsg(pose);
return publishCollisionCuboid(pose_msg, width, depth, height, name, color);
return publishCollisionCuboid(convertPose(block_pose), width, depth, height, name, color);
}

bool MoveItVisualTools::publishCollisionCuboid(const moveit_msgs::CollisionObject& collision_obj,
const rviz_visual_tools::colors& color)

bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Pose& pose, double width, double depth,
bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Pose& block_pose, double width, double depth,
double height, const std::string& name,
const rviz_visual_tools::colors& color)
{
return processCollisionObjectMsg(createCollisionObject(pose, width, depth, height, name, color), color);
return processCollisionObjectMsg(createCuboidCollisionObjectMsg(block_pose, width, depth, height, name), color);
}

bool MoveItVisualTools::publishCollisionFloor(double z, const std::string& plane_name,
Expand Down

0 comments on commit b274866

Please sign in to comment.