Skip to content

Commit 3885dc0

Browse files
author
Mike Lautman
committed
separate collision_object creation from publishing
1 parent 883beb7 commit 3885dc0

File tree

2 files changed

+69
-22
lines changed

2 files changed

+69
-22
lines changed

‎include/moveit_visual_tools/moveit_visual_tools.h‎

Lines changed: 14 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -323,33 +323,40 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
323323
const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
324324

325325
/**
326-
* \brief Create a MoveIt Collision block at the given pose
326+
* \brief Create/Publish a MoveIt Collision block at the given pose
327327
* \param pose - location of center of block
328328
* \param name - semantic name of MoveIt collision object
329329
* \param size - height=width=depth=size
330330
* \param color to display the collision object with
331331
* \return true on sucess
332332
**/
333+
moveit_msgs::CollisionObject createCollisionBlock(const geometry_msgs::Pose& block_pose, const std::string& name,
334+
double block_size);
335+
333336
bool publishCollisionBlock(const geometry_msgs::Pose& block_pose, const std::string& block_name = "block",
334337
double block_size = 0.1,
335338
const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
336339

337340
/**
338-
* \brief Create a MoveIt collision rectangular cuboid at the given pose
341+
* \brief Create/Publish a MoveIt collision rectangular cuboid at the given pose
339342
* \param point1 - top left of rectangle
340343
* \param point2 - bottom right of rectangle
341344
* \param name - semantic name of MoveIt collision object
342345
* \param color to display the collision object with
343346
* \return true on sucess
344347
**/
348+
moveit_msgs::CollisionObject createCollisionObject(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2,
349+
const std::string& name);
350+
moveit_msgs::CollisionObject createCollisionObject(const geometry_msgs::Point& point1,
351+
const geometry_msgs::Point& point2, const std::string& name);
345352
bool publishCollisionCuboid(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, const std::string& name,
346353
const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
347354
bool publishCollisionCuboid(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2,
348355
const std::string& name,
349356
const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
350357

351358
/**
352-
* \brief Create a MoveIt collision rectangular cuboid at the given pose
359+
* \brief Create/Publish a MoveIt collision rectangular cuboid at the given pose
353360
* \param pose - position of the centroid of the cube
354361
* \param width - width of the object in its local frame
355362
* \param depth - depth of the object in its local frame
@@ -358,6 +365,10 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
358365
* \param color to display the collision object with
359366
* \return true on sucess
360367
**/
368+
moveit_msgs::CollisionObject createCollisionObject(const Eigen::Isometry3d& pose, double width, double depth,
369+
double height, const std::string& name);
370+
moveit_msgs::CollisionObject createCollisionObject(const geometry_msgs::Pose& pose, double width, double depth,
371+
double height, const std::string& name);
361372
bool publishCollisionCuboid(const Eigen::Isometry3d& pose, double width, double depth, double height,
362373
const std::string& name, const rviz_visual_tools::colors& color);
363374

‎src/moveit_visual_tools.cpp‎

Lines changed: 55 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -618,8 +618,8 @@ bool MoveItVisualTools::attachCO(const std::string& name, const std::string& ee_
618618
return processAttachedCollisionObjectMsg(aco);
619619
}
620620

621-
bool MoveItVisualTools::publishCollisionBlock(const geometry_msgs::Pose& block_pose, const std::string& name,
622-
double block_size, const rviz_visual_tools::colors& color)
621+
moveit_msgs::CollisionObject MoveItVisualTools::createCollisionBlock(const geometry_msgs::Pose& block_pose,
622+
const std::string& name, double block_size)
623623
{
624624
moveit_msgs::CollisionObject collision_obj;
625625
collision_obj.header.stamp = ros::Time::now();
@@ -637,19 +637,18 @@ bool MoveItVisualTools::publishCollisionBlock(const geometry_msgs::Pose& block_p
637637
collision_obj.primitive_poses.resize(1);
638638
collision_obj.primitive_poses[0] = block_pose;
639639

640-
// ROS_INFO_STREAM_NAMED(LOGNAME,"CollisionObject: \n " << collision_obj);
641-
// ROS_DEBUG_STREAM_NAMED(LOGNAME,"Published collision object " << name);
642-
return processCollisionObjectMsg(collision_obj, color);
640+
return collision_obj;
643641
}
644642

645-
bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2,
646-
const std::string& name, const rviz_visual_tools::colors& color)
643+
bool MoveItVisualTools::publishCollisionBlock(const geometry_msgs::Pose& block_pose, const std::string& name,
644+
double block_size, const rviz_visual_tools::colors& color)
647645
{
648-
return publishCollisionCuboid(convertPoint(point1), convertPoint(point2), name, color);
646+
return processCollisionObjectMsg(createCollisionBlock(block_pose, name, block_size), color);
649647
}
650648

651-
bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2,
652-
const std::string& name, const rviz_visual_tools::colors& color)
649+
moveit_msgs::CollisionObject MoveItVisualTools::createCollisionObject(const geometry_msgs::Point& point1,
650+
const geometry_msgs::Point& point2,
651+
const std::string& name)
653652
{
654653
moveit_msgs::CollisionObject collision_obj;
655654
collision_obj.header.stamp = ros::Time::now();
@@ -680,20 +679,32 @@ bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Point& point
680679
if (!collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z])
681680
collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = rviz_visual_tools::SMALL_SCALE;
682681

683-
// ROS_INFO_STREAM_NAMED(LOGNAME,"CollisionObject: \n " << collision_obj);
684-
return processCollisionObjectMsg(collision_obj, color);
682+
return collision_obj;
685683
}
686684

687-
bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Isometry3d& pose, double width, double depth, double height,
685+
moveit_msgs::CollisionObject MoveItVisualTools::createCollisionObject(const Eigen::Vector3d& point1,
686+
const Eigen::Vector3d& point2,
687+
const std::string& name)
688+
{
689+
return createCollisionObject(convertPoint(point1), convertPoint(point2), name);
690+
}
691+
692+
bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2,
688693
const std::string& name, const rviz_visual_tools::colors& color)
689694
{
690-
geometry_msgs::Pose pose_msg = tf2::toMsg(pose);
691-
return publishCollisionCuboid(pose_msg, width, depth, height, name, color);
695+
return publishCollisionCuboid(convertPoint(point1), convertPoint(point2), name, color);
692696
}
693697

694-
bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Pose& pose, double width, double depth,
695-
double height, const std::string& name,
696-
const rviz_visual_tools::colors& color)
698+
bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2,
699+
const std::string& name, const rviz_visual_tools::colors& color)
700+
{
701+
// ROS_INFO_STREAM_NAMED(LOGNAME,"CollisionObject: \n " << collision_obj);
702+
return processCollisionObjectMsg(createCollisionObject(point1, point2, name), color);
703+
}
704+
705+
moveit_msgs::CollisionObject MoveItVisualTools::createCollisionObject(const geometry_msgs::Pose& pose, double width,
706+
double depth, double height,
707+
const std::string& name)
697708
{
698709
moveit_msgs::CollisionObject collision_obj;
699710
collision_obj.header.stamp = ros::Time::now();
@@ -722,7 +733,32 @@ bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Pose& pose,
722733
if (!collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z])
723734
collision_obj.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] = rviz_visual_tools::SMALL_SCALE;
724735

725-
return processCollisionObjectMsg(collision_obj, color);
736+
return collision_obj;
737+
}
738+
739+
moveit_msgs::CollisionObject MoveItVisualTools::createCollisionObject(const Eigen::Isometry3d& pose, double width,
740+
double depth, double height,
741+
const std::string& name)
742+
{
743+
geometry_msgs::Pose pose_msg = tf2::toMsg(pose);
744+
return createCollisionObject(pose_msg, width, depth, height, name, color);
745+
}
746+
747+
bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Isometry3d& pose, double width, double depth, double height,
748+
const std::string& name, const rviz_visual_tools::colors& color)
749+
{
750+
geometry_msgs::Pose pose_msg = tf2::toMsg(pose);
751+
return publishCollisionCuboid(pose_msg, width, depth, height, name, color);
752+
}
753+
754+
bool MoveItVisualTools::publishCollisionCuboid(const moveit_msgs::CollisionObject& collision_obj,
755+
const rviz_visual_tools::colors& color)
756+
757+
bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Pose& pose, double width, double depth,
758+
double height, const std::string& name,
759+
const rviz_visual_tools::colors& color)
760+
{
761+
return processCollisionObjectMsg(createCollisionObject(pose, width, depth, height, name, color), color);
726762
}
727763

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

0 commit comments

Comments
 (0)