Skip to content

separate collision_object creation from publishing #55

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

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 1 commit
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
17 changes: 14 additions & 3 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 @@ -323,33 +323,40 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);

/**
* \brief Create a MoveIt Collision block at the given pose
* \brief Create/Publish a MoveIt Collision block at the given pose
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

i'd rather you copy this doxygen for reach function. it seems these two functions due fairly different things, and the \brief should explain that difference better.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

* \param pose - location of center of block
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

block_pose

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

* \param name - semantic name of MoveIt collision object
* \param size - height=width=depth=size
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

rename to block size

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

* \param color to display the collision object with
* \return true on sucess
**/
moveit_msgs::CollisionObject createCollisionBlock(const geometry_msgs::Pose& block_pose, const std::string& name,
double block_size);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

is it possible for the CollisionObject msgs type to support color as well?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

we would have to change the msg


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 a MoveIt collision rectangular cuboid at the given pose
* \brief Create/Publish a MoveIt collision rectangular cuboid at the given pose
* \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,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this should be createCollisionCuboid right? Even better: createCollisionCuboidMsg

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

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 a MoveIt collision rectangular cuboid at the given pose
* \brief Create/Publish a MoveIt collision rectangular cuboid at the given pose
* \param 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
Expand All @@ -358,6 +365,10 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
* \param color to display the collision object with
* \return true on sucess
**/
moveit_msgs::CollisionObject createCollisionObject(const Eigen::Isometry3d& pose, double width, double depth,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

same comments:
rename function to include Cuboid

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

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,
const std::string& name, const rviz_visual_tools::colors& color);

Expand Down
74 changes: 55 additions & 19 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,8 @@ 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::createCollisionBlock(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 +637,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(createCollisionBlock(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::createCollisionObject(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,20 +679,32 @@ 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;
}

bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Isometry3d& pose, double width, double depth, double height,
moveit_msgs::CollisionObject MoveItVisualTools::createCollisionObject(const Eigen::Vector3d& point1,
const Eigen::Vector3d& point2,
const std::string& name)
{
return createCollisionObject(convertPoint(point1), convertPoint(point2), name);
}

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)
{
// ROS_INFO_STREAM_NAMED(LOGNAME,"CollisionObject: \n " << collision_obj);
return processCollisionObjectMsg(createCollisionObject(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 collision_obj;
collision_obj.header.stamp = ros::Time::now();
Expand Down Expand Up @@ -722,7 +733,32 @@ 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::createCollisionObject(const Eigen::Isometry3d& pose, double width,
double depth, double height,
const std::string& name)
{
geometry_msgs::Pose pose_msg = tf2::toMsg(pose);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

typically in *_visual_tools, we use the built-in converter helper function rather than tf2, could you use that? e.g. convertPose()

same below

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

return createCollisionObject(pose_msg, width, depth, height, name, color);
}

bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Isometry3d& 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);
}

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,
double height, const std::string& name,
const rviz_visual_tools::colors& color)
{
return processCollisionObjectMsg(createCollisionObject(pose, width, depth, height, name, color), color);
}

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