Skip to content

Commit 6c354a5

Browse files
author
Mike Lautman
committed
pr fixup
1 parent 3885dc0 commit 6c354a5

File tree

2 files changed

+72
-50
lines changed

2 files changed

+72
-50
lines changed

‎include/moveit_visual_tools/moveit_visual_tools.h‎

Lines changed: 45 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -323,56 +323,82 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
323323
const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
324324

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

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

340347
/**
341-
* \brief Create/Publish a MoveIt collision rectangular cuboid at the given pose
348+
* \brief Create a cuboid MoveIt collision object msg
349+
* \param point1 - top left of rectangle
350+
* \param point2 - bottom right of rectangle
351+
* \param name - semantic name of MoveIt collision object
352+
* \return moveit_msgs::CollisionObject
353+
**/
354+
moveit_msgs::CollisionObject createCuboidCollisionObjectMsg(const Eigen::Vector3d& point1,
355+
const Eigen::Vector3d& point2, const std::string& name);
356+
moveit_msgs::CollisionObject createCuboidCollisionObjectMsg(const geometry_msgs::Point& point1,
357+
const geometry_msgs::Point& point2,
358+
const std::string& name);
359+
360+
/**
361+
* \brief Publish a cuboid MoveIt collision object between two points
342362
* \param point1 - top left of rectangle
343363
* \param point2 - bottom right of rectangle
344364
* \param name - semantic name of MoveIt collision object
345365
* \param color to display the collision object with
346366
* \return true on sucess
347367
**/
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);
352368
bool publishCollisionCuboid(const Eigen::Vector3d& point1, const Eigen::Vector3d& point2, const std::string& name,
353369
const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
354370
bool publishCollisionCuboid(const geometry_msgs::Point& point1, const geometry_msgs::Point& point2,
355371
const std::string& name,
356372
const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
357373

358374
/**
359-
* \brief Create/Publish a MoveIt collision rectangular cuboid at the given pose
360-
* \param pose - position of the centroid of the cube
375+
* \brief Create a cuboid MoveIt collision object msg
376+
* \param block_pose - position of the centroid of the cube
377+
* \param width - width of the object in its local frame
378+
* \param depth - depth of the object in its local frame
379+
* \param height - height of the object in its local frame
380+
* \param name - semantic name of MoveIt collision object
381+
* \return moveit_msgs::CollisionObject
382+
**/
383+
moveit_msgs::CollisionObject createCuboidCollisionObjectMsg(const Eigen::Isometry3d& block_pose, double width,
384+
double depth, double height, const std::string& name);
385+
moveit_msgs::CollisionObject createCuboidCollisionObjectMsg(const geometry_msgs::Pose& block_pose, double width,
386+
double depth, double height, const std::string& name);
387+
388+
/**
389+
* \brief Publish a cuboid MoveIt collision object at the given pose
390+
* \param block_pose - position of the centroid of the cube
361391
* \param width - width of the object in its local frame
362392
* \param depth - depth of the object in its local frame
363393
* \param height - height of the object in its local frame
364394
* \param name - semantic name of MoveIt collision object
365395
* \param color to display the collision object with
366396
* \return true on sucess
367397
**/
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);
372-
bool publishCollisionCuboid(const Eigen::Isometry3d& pose, double width, double depth, double height,
398+
bool publishCollisionCuboid(const Eigen::Isometry3d& block_pose, double width, double depth, double height,
373399
const std::string& name, const rviz_visual_tools::colors& color);
374400

375-
bool publishCollisionCuboid(const geometry_msgs::Pose& pose, double width, double depth, double height,
401+
bool publishCollisionCuboid(const geometry_msgs::Pose& block_pose, double width, double depth, double height,
376402
const std::string& name, const rviz_visual_tools::colors& color);
377403

378404
/**

‎src/moveit_visual_tools.cpp‎

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

621-
moveit_msgs::CollisionObject MoveItVisualTools::createCollisionBlock(const geometry_msgs::Pose& block_pose,
622-
const std::string& name, double block_size)
621+
moveit_msgs::CollisionObject MoveItVisualTools::createBlockCollisionObjectMsg(const geometry_msgs::Pose& block_pose,
622+
const std::string& name,
623+
double block_size)
623624
{
624625
moveit_msgs::CollisionObject collision_obj;
625626
collision_obj.header.stamp = ros::Time::now();
@@ -643,12 +644,12 @@ moveit_msgs::CollisionObject MoveItVisualTools::createCollisionBlock(const geome
643644
bool MoveItVisualTools::publishCollisionBlock(const geometry_msgs::Pose& block_pose, const std::string& name,
644645
double block_size, const rviz_visual_tools::colors& color)
645646
{
646-
return processCollisionObjectMsg(createCollisionBlock(block_pose, name, block_size), color);
647+
return processCollisionObjectMsg(createBlockCollisionObjectMsg(block_pose, name, block_size), color);
647648
}
648649

649-
moveit_msgs::CollisionObject MoveItVisualTools::createCollisionObject(const geometry_msgs::Point& point1,
650-
const geometry_msgs::Point& point2,
651-
const std::string& name)
650+
moveit_msgs::CollisionObject MoveItVisualTools::createCuboidCollisionObjectMsg(const geometry_msgs::Point& point1,
651+
const geometry_msgs::Point& point2,
652+
const std::string& name)
652653
{
653654
moveit_msgs::CollisionObject collision_obj;
654655
collision_obj.header.stamp = ros::Time::now();
@@ -682,11 +683,11 @@ moveit_msgs::CollisionObject MoveItVisualTools::createCollisionObject(const geom
682683
return collision_obj;
683684
}
684685

685-
moveit_msgs::CollisionObject MoveItVisualTools::createCollisionObject(const Eigen::Vector3d& point1,
686-
const Eigen::Vector3d& point2,
687-
const std::string& name)
686+
moveit_msgs::CollisionObject MoveItVisualTools::createCuboidCollisionObjectMsg(const Eigen::Vector3d& point1,
687+
const Eigen::Vector3d& point2,
688+
const std::string& name)
688689
{
689-
return createCollisionObject(convertPoint(point1), convertPoint(point2), name);
690+
return createCuboidCollisionObjectMsg(convertPoint(point1), convertPoint(point2), name);
690691
}
691692

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

705-
moveit_msgs::CollisionObject MoveItVisualTools::createCollisionObject(const geometry_msgs::Pose& pose, double width,
706-
double depth, double height,
707-
const std::string& name)
705+
moveit_msgs::CollisionObject MoveItVisualTools::createCuboidCollisionObjectMsg(const geometry_msgs::Pose& block_pose,
706+
double width, double depth,
707+
double height, const std::string& name)
708708
{
709709
moveit_msgs::CollisionObject collision_obj;
710710
collision_obj.header.stamp = ros::Time::now();
711711
collision_obj.header.frame_id = base_frame_;
712712
collision_obj.id = name;
713713
collision_obj.operation = moveit_msgs::CollisionObject::ADD;
714714

715-
// Calculate center pose
715+
// Calculate center block_pose
716716
collision_obj.primitive_poses.resize(1);
717-
collision_obj.primitive_poses[0] = pose;
717+
collision_obj.primitive_poses[0] = block_pose;
718718

719719
// Calculate scale
720720
collision_obj.primitives.resize(1);
@@ -736,29 +736,25 @@ moveit_msgs::CollisionObject MoveItVisualTools::createCollisionObject(const geom
736736
return collision_obj;
737737
}
738738

739-
moveit_msgs::CollisionObject MoveItVisualTools::createCollisionObject(const Eigen::Isometry3d& pose, double width,
740-
double depth, double height,
741-
const std::string& name)
739+
moveit_msgs::CollisionObject MoveItVisualTools::createCuboidCollisionObjectMsg(const Eigen::Isometry3d& block_pose,
740+
double width, double depth,
741+
double height, const std::string& name)
742742
{
743-
geometry_msgs::Pose pose_msg = tf2::toMsg(pose);
744-
return createCollisionObject(pose_msg, width, depth, height, name, color);
743+
return createCuboidCollisionObjectMsg(convertPose(pose_msg), width, depth, height, name);
745744
}
746745

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)
746+
bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Isometry3d& block_pose, double width, double depth,
747+
double height, const std::string& name,
748+
const rviz_visual_tools::colors& color)
749749
{
750-
geometry_msgs::Pose pose_msg = tf2::toMsg(pose);
751-
return publishCollisionCuboid(pose_msg, width, depth, height, name, color);
750+
return publishCollisionCuboid(convertPose(block_pose), width, depth, height, name, color);
752751
}
753752

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,
753+
bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Pose& block_pose, double width, double depth,
758754
double height, const std::string& name,
759755
const rviz_visual_tools::colors& color)
760756
{
761-
return processCollisionObjectMsg(createCollisionObject(pose, width, depth, height, name, color), color);
757+
return processCollisionObjectMsg(createCuboidCollisionObjectMsg(block_pose, width, depth, height, name), color);
762758
}
763759

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

0 commit comments

Comments
 (0)