Contains various utility functions for RViz visualization, conversions, transformations and robot controllers. More...

Enumerations
enum	SHAPE_PRIMITIVES { SHAPE_PRIMITIVES::BOX, SHAPE_PRIMITIVES::CYLINDER, SHAPE_PRIMITIVES::CONE, SHAPE_PRIMITIVES::SPHERE }
	Primitive shapes used for RViz objects. More...

Functions
Eigen::Quaternionf	ypr2quat (float y, float p, float r)
	Conversion from YPR to Quaternion. More...

void	quat2ypr (tf::Quaternion &q, double &r, double &p, double &y)
	Conversion from Quaternion to YPR. More...

template<typename F >
F	deg2rad (F deg)
	Conversion from degree to radians. More...

template<typename F >
F	rad2deg (F rad)
	Conversion from radians to degrees. More...

void	createCollisionObjectFromPrimitive (moveit_msgs::CollisionObject &collision_obj, std::string primitive_id, shape_msgs::SolidPrimitive &primitive, geometry_msgs::Pose &obj_pose, shadow_utils::SHAPE_PRIMITIVES primitive_type, const std::vector< double > &primitive_dims)
	Create collision object for the planning scene, used by MoveIt planning. More...

void	createAttachedObjectFromPrimitive (moveit_msgs::AttachedCollisionObject &attached_obj, std::string primitive_id, shape_msgs::SolidPrimitive &primitive, geometry_msgs::Pose &obj_pose, shadow_utils::SHAPE_PRIMITIVES primitive_type, const std::vector< double > &primitive_dims)
	Create attached object for the planning scene, used by MoveIt planning when attaching object to robot This helps plan when an object is "attached" to the end of a robot, after grasping. More...

void	addCollisionObjectToScene (moveit_msgs::CollisionObject &collision_obj, moveit_msgs::PlanningScene &planning_scene_msg, ros::Publisher &planning_scene_diff_publisher)
	Add a collision object to the planning scene. More...

void	addAttachedObjectToScene (moveit_msgs::AttachedCollisionObject &attached_obj, moveit_msgs::PlanningScene &planning_scene_msg, ros::Publisher &planning_scene_diff_publisher)
	Add a collision object to the planning scene. More...

void	moveCollisionObjectInScene (moveit_msgs::CollisionObject &collision_obj, geometry_msgs::Pose &obj_pose, moveit_msgs::PlanningScene &planning_scene_msg, ros::Publisher &planning_scene_diff_publisher)
	Move an existing collision object in the planning scene. More...

void	attachObjectToRobot (moveit_msgs::AttachedCollisionObject &attached_obj, moveit_msgs::PlanningScene &planning_scene_msg, ros::Publisher &planning_scene_diff_publisher)
	Attach a collision object to the robot. More...

void	detachObjectFromRobot (moveit_msgs::AttachedCollisionObject &attached_obj, moveit_msgs::PlanningScene &planning_scene_msg, ros::Publisher &planning_scene_diff_publisher)
	Detach an attached collision object from the robot. More...

void	removeAllObjectsFromScene (moveit::planning_interface::PlanningSceneInterface &planning_scene_interface)
	Remove all objects from the scene. More...

void	removeCollisionObjectFromScene (moveit::planning_interface::PlanningSceneInterface &planning_scene_interface, std::string obj_id)
	Remove some object from the scene. More...

void	getPoseFromPositionYPR (geometry_msgs::Pose &pose, float x, float y, float z, float yaw, float pitch, float roll)
	Generate ROS Pose from position and YPR. More...

void	getPoseFromPositionQuaternion (geometry_msgs::Pose &pose, float x, float y, float z, float qx, float qy, float qz, float qw)
	Generate ROS Pose from position and quaternion. More...

void	getPoseFromPositionYPR (geometry_msgs::Pose &pose, float x, float y, float z, tf::Vector3 &v)

void	getPoseFromPositionQuaternion (geometry_msgs::Pose &pose, float x, float y, float z, tf::Quaternion &q)

void	getPoseFromPositionQuaternion (geometry_msgs::Pose &pose, tf::StampedTransform &tf, tf::Quaternion &q)

void	getTfFromFrames (tf::TransformListener &listener, tf::StampedTransform &tf, std::string parent_frame, std::string child_frame)
	Get TF from parent and child frames. More...

std::string	getControllerTopic (std::string &joint_name)
	Get the name of the controller responsible for controlling the given joint name. More...

std::vector< ros::Publisher >	createJointControllerPublishers (std::vector< std::string > &joint_names, ros::NodeHandle &n)
	Create a vector of publishers for each given joint name, using the ROS node handle. More...

void	broadcastTransformForGrasp (std::vector< double > xyzrpy, const std::string frame_name)
	Broadcasts a transform from the wrist to given frame, which can be used for planning targets for finger. More...

geometry_msgs::Pose	getPoseBetweenFrames (const std::string parent_frame, const std::string child_frame)
	Get the pose between two frames. More...

moveit_msgs::RobotState	computeJointsFromPose (moveit::planning_interface::MoveGroupInterface &move_group_interface, double timeout, int num_attempts, geometry_msgs::Pose &target_pose)
	Creates a RobotState using the inverse kinematics solver, given a target pose. Uses a service call to the "compute_ik" service to obtain the joint angles needed for the given MoveGroup to reach the specified target pose. More...

void	startTrajectoryController (ros::NodeHandle n, std::string controller_name)
	Starts trajectory controller for arm or hand, using ControllerManager service call. More...

void	stopTrajectoryController (ros::NodeHandle n, std::string controller_name)
	Stops trajectory controller for arm or hand, using ControllerManager service call. More...

Detailed Description

Contains various utility functions for RViz visualization, conversions, transformations and robot controllers.

shadow_utils.hpp "include/shadowlibs/shadow_utils.hpp"

Enumeration Type Documentation

enum shadow_utils::SHAPE_PRIMITIVES

strong

Primitive shapes used for RViz objects.

Enumerator
BOX
CYLINDER
CONE
SPHERE

Function Documentation

void shadow_utils::addAttachedObjectToScene	(	moveit_msgs::AttachedCollisionObject &	attached_obj,
		moveit_msgs::PlanningScene &	planning_scene_msg,
		ros::Publisher &	planning_scene_diff_publisher
	)

Add a collision object to the planning scene.

Parameters

attached_obj	The attached object to add to scene
planning_scene_msg	The PlanningScene to add object to
planning_scene_diff_publisher	The publisher for updating the scene

void shadow_utils::addCollisionObjectToScene	(	moveit_msgs::CollisionObject &	collision_obj,
		moveit_msgs::PlanningScene &	planning_scene_msg,
		ros::Publisher &	planning_scene_diff_publisher
	)

Add a collision object to the planning scene.

Parameters

collision_obj	The object to add to scene
planning_scene_msg	The PlanningScene to add object to
planning_scene_diff_publisher	The publisher for updating the scene

void shadow_utils::attachObjectToRobot	(	moveit_msgs::AttachedCollisionObject &	attached_obj,
		moveit_msgs::PlanningScene &	planning_scene_msg,
		ros::Publisher &	planning_scene_diff_publisher
	)

Attach a collision object to the robot.

Parameters

attached_obj	The collision object to attach
planning_scene_msg	The PlanningScene to add object to
planning_scene_diff_publisher	The publisher for updating the scene

void shadow_utils::broadcastTransformForGrasp	(	std::vector< double >	xyzrpy,
		const std::string	frame_name
	)

Broadcasts a transform from the wrist to given frame, which can be used for planning targets for finger.

Parameters

xyzrpy	The pose of the transform
frame_name	The frame name of the transform

moveit_msgs::RobotState shadow_utils::computeJointsFromPose	(	moveit::planning_interface::MoveGroupInterface &	move_group_interface,
		double	timeout,
		int	num_attempts,
		geometry_msgs::Pose &	target_pose
	)

Creates a RobotState using the inverse kinematics solver, given a target pose. Uses a service call to the "compute_ik" service to obtain the joint angles needed for the given MoveGroup to reach the specified target pose.

See also: http://docs.ros.org/kinetic/api/moveit_msgs/html/msg/RobotState.html; http://docs.ros.org/kinetic/api/moveit_msgs/html/srv/GetPositionIK.html

Parameters

move_group_interface	The MoveGroup to compute RobotState for
timeout	The timeout for computation
num_attempts	The number of tries for computing IK
target_pose	The target pose to compute joint angles for

Returns: The RobotState computed by the IK service

void shadow_utils::createAttachedObjectFromPrimitive	(	moveit_msgs::AttachedCollisionObject &	attached_obj,
		std::string	primitive_id,
		shape_msgs::SolidPrimitive &	primitive,
		geometry_msgs::Pose &	obj_pose,
		shadow_utils::SHAPE_PRIMITIVES	primitive_type,
		const std::vector< double > &	primitive_dims
	)

Create attached object for the planning scene, used by MoveIt planning when attaching object to robot This helps plan when an object is "attached" to the end of a robot, after grasping.

Parameters

attached_obj	The MoveIt attached collision object
primitive_id	The ID of the attached object
primitive	The primitive used by the attached object
obj_pose	The pose of the attached object
primitive_type	The shape of the attached object
primitive_dims	The dimension of the attached object

void shadow_utils::createCollisionObjectFromPrimitive	(	moveit_msgs::CollisionObject &	collision_obj,
		std::string	primitive_id,
		shape_msgs::SolidPrimitive &	primitive,
		geometry_msgs::Pose &	obj_pose,
		shadow_utils::SHAPE_PRIMITIVES	primitive_type,
		const std::vector< double > &	primitive_dims
	)

Create collision object for the planning scene, used by MoveIt planning.

Parameters

collision_obj	The MoveIt collision object
primitive_id	The ID of the collision object
primitive	The primitive used by the collision object
obj_pose	The pose of the collision object
primitive_type	The shape of the collision object
primitive_dims	The dimension of the collision object

std::vector< ros::Publisher > shadow_utils::createJointControllerPublishers	(	std::vector< std::string > &	joint_names,
		ros::NodeHandle &	n
	)

Create a vector of publishers for each given joint name, using the ROS node handle.

Parameters

joint_names	Names of joints to create controllers for
n	The ROS node handle

Returns: Vector of publishers for each joint given as input

template<typename F >

F shadow_utils::deg2rad ( F deg )

inline

Conversion from degree to radians.

void shadow_utils::detachObjectFromRobot	(	moveit_msgs::AttachedCollisionObject &	attached_obj,
		moveit_msgs::PlanningScene &	planning_scene_msg,
		ros::Publisher &	planning_scene_diff_publisher
	)

Detach an attached collision object from the robot.

Parameters

attached_obj	The collision object to detach
planning_scene_msg	The PlanningScene to add object to
planning_scene_diff_publisher	The publisher for updating the scene

std::string shadow_utils::getControllerTopic ( std::string & joint_name )

Get the name of the controller responsible for controlling the given joint name.

Parameters

joint_name Name of joint to fetch controller for

Returns: Name of the controller publisher

geometry_msgs::Pose shadow_utils::getPoseBetweenFrames	(	const std::string	parent_frame,
		const std::string	child_frame
	)

Get the pose between two frames.

Parameters

parent_frame	The parent frame
child_frame	The child frame

Returns: The pose between parent and child frames

void shadow_utils::getPoseFromPositionQuaternion	(	geometry_msgs::Pose &	pose,
		float	x,
		float	y,
		float	z,
		float	qx,
		float	qy,
		float	qz,
		float	qw
	)

Generate ROS Pose from position and quaternion.

Parameters

pose	The pose to be constructed
x	X position
y	Y position
z	Z position
qx	x quaternion
qy	y quaternion
qz	z quaternion
qw	w quaternion

void shadow_utils::getPoseFromPositionQuaternion	(	geometry_msgs::Pose &	pose,
		float	x,
		float	y,
		float	z,
		tf::Quaternion &	q
	)

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

void shadow_utils::getPoseFromPositionQuaternion	(	geometry_msgs::Pose &	pose,
		tf::StampedTransform &	tf,
		tf::Quaternion &	q
	)

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

void shadow_utils::getPoseFromPositionYPR	(	geometry_msgs::Pose &	pose,
		float	x,
		float	y,
		float	z,
		float	yaw,
		float	pitch,
		float	roll
	)

Generate ROS Pose from position and YPR.

Parameters

pose	The pose to be constructed
x	X position
y	Y position
z	Z position
yaw	Yaw orientation
pitch	Pitch orientation
roll	Roll orientation

void shadow_utils::getPoseFromPositionYPR	(	geometry_msgs::Pose &	pose,
		float	x,
		float	y,
		float	z,
		tf::Vector3 &	v
	)

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

void shadow_utils::getTfFromFrames	(	tf::TransformListener &	listener,
		tf::StampedTransform &	tf,
		std::string	parent_frame,
		std::string	child_frame
	)

Get TF from parent and child frames.

Parameters

listener	The TF listener
tf	The transform data obtained
parent_frame	The name of the parent frame
child_frame	The name of the child frame

void shadow_utils::moveCollisionObjectInScene	(	moveit_msgs::CollisionObject &	collision_obj,
		geometry_msgs::Pose &	obj_pose,
		moveit_msgs::PlanningScene &	planning_scene_msg,
		ros::Publisher &	planning_scene_diff_publisher
	)

Move an existing collision object in the planning scene.

Parameters

collision_obj	The collision object to move in the scene
obj_pose	The pose of the collision object in the scene
planning_scene_msg	The PlanningScene to add object to
planning_scene_diff_publisher	The publisher for updating the scene

void shadow_utils::quat2ypr	(	tf::Quaternion &	q,
		double &	r,
		double &	p,
		double &	y
	)

Conversion from Quaternion to YPR.

template<typename F >

F shadow_utils::rad2deg ( F rad )

inline

Conversion from radians to degrees.

void shadow_utils::removeAllObjectsFromScene ( moveit::planning_interface::PlanningSceneInterface & planning_scene_interface )

Remove all objects from the scene.

Parameters

planning_scene_interface The PlanningScene to remove objects from

void shadow_utils::removeCollisionObjectFromScene	(	moveit::planning_interface::PlanningSceneInterface &	planning_scene_interface,
		std::string	obj_id
	)

Remove some object from the scene.

Parameters

planning_scene_interface	The PlanningScene to remove objects from
obj_id	The ID of the object to be removed

void shadow_utils::startTrajectoryController	(	ros::NodeHandle	n,
		std::string	controller_name
	)

Starts trajectory controller for arm or hand, using ControllerManager service call.

Parameters

n	- ROS node handle
controller_name	- Name of controller, arm or hand (rh_trajectory_controller OR ra_trajectory_controller)

void shadow_utils::stopTrajectoryController	(	ros::NodeHandle	n,
		std::string	controller_name
	)

Stops trajectory controller for arm or hand, using ControllerManager service call.

Parameters

n	- ROS node handle
controller_name	- Name of controller, arm or hand (rh_trajectory_controller OR ra_trajectory_controller)

Eigen::Quaternionf shadow_utils::ypr2quat	(	float	y,
		float	p,
		float	r
	)

Conversion from YPR to Quaternion.

Enumerations

Functions

Detailed Description

Enumeration Type Documentation

Function Documentation