Skip to content
New issue

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

Sign up for GitHub

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

Already on GitHub? Sign in to your account

separate collision_object creation from publishing #55

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

separate collision_object creation from publishing #55

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
3885dc0
separate collision_object creation from publishing
Aug 12, 2019
b274866
pr fixup
Aug 18, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
51 changes: 44 additions & 7 deletions include/moveit_visual_tools/moveit_visual_tools.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -324,9 +324,19 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools

/**
* \brief Create a MoveIt Collision block at the given pose
* \param pose - location of center of block
* \param block_pose - location of center of block
* \param name - semantic name of MoveIt collision object
* \param size - height=width=depth=size
* \param size - height=width=depth=block_size
* \return true on sucess
**/
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
**/
Expand All @@ -335,7 +345,20 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);

/**
* \brief Create 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
Expand All @@ -349,19 +372,33 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);

/**
* \brief Create 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
**/
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
74 changes: 53 additions & 21 deletions src/moveit_visual_tools.cpp
View file
Open in desktop
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);
}

bool MoveItVisualTools::publishCollisionBlock(const geometry_msgs::Pose& block_pose, const std::string& name,
double block_size, const rviz_visual_tools::colors& color)
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 @@ -637,19 +638,18 @@ bool MoveItVisualTools::publishCollisionBlock(const geometry_msgs::Pose& block_p
collision_obj.primitive_poses.resize(1);
collision_obj.primitive_poses[0] = block_pose;

// ROS_INFO_STREAM_NAMED(LOGNAME,"CollisionObject: \n " << collision_obj);
// ROS_DEBUG_STREAM_NAMED(LOGNAME,"Published collision object " << name);
return processCollisionObjectMsg(collision_obj, color);
return collision_obj;
}

bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2,
const std::string& name, const rviz_visual_tools::colors& color)
bool MoveItVisualTools::publishCollisionBlock(const geometry_msgs::Pose& block_pose, const std::string& name,
double block_size, const rviz_visual_tools::colors& color)
{
return publishCollisionCuboid(convertPoint(point1), convertPoint(point2), name, color);
return processCollisionObjectMsg(createBlockCollisionObjectMsg(block_pose, name, block_size), color);
}

bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2,
const std::string& name, const rviz_visual_tools::colors& color)
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 @@ -680,30 +680,41 @@ bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Point& point
if (!collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z])
collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = rviz_visual_tools::SMALL_SCALE;

// ROS_INFO_STREAM_NAMED(LOGNAME,"CollisionObject: \n " << collision_obj);
return processCollisionObjectMsg(collision_obj, color);
return collision_obj;
}

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

bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Isometry3d& pose, double width, double depth, double height,
bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2,
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(convertPoint(point1), convertPoint(point2), name, color);
}

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

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 @@ -722,7 +733,28 @@ bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Pose& pose,
if (!collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z])
collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = rviz_visual_tools::SMALL_SCALE;

return processCollisionObjectMsg(collision_obj, color);
return collision_obj;
}

moveit_msgs::CollisionObject MoveItVisualTools::createCuboidCollisionObjectMsg(const Eigen::Isometry3d& block_pose,
double width, double depth,
double height, const std::string& name)
{
return createCuboidCollisionObjectMsg(convertPose(block_pose), width, depth, height, name);
}

bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Isometry3d& block_pose, double width, double depth,
double height, const std::string& name,
const rviz_visual_tools::colors& color)
{
return publishCollisionCuboid(convertPose(block_pose), width, depth, height, name, color);
}

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(createCuboidCollisionObjectMsg(block_pose, width, depth, height, name), color);
}

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