Message handler that streams joint trajectories to the robot controller. Contains FS100-specific motion control commands. More...

#include <joint_trajectory_streamer.h>

Inheritance diagram for motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer:

[legend]

Public Member Functions
virtual bool	create_message (int seq, const trajectory_msgs::JointTrajectoryPoint &pt, SimpleMessage *msg)
	Create SimpleMessage for sending to the robot. More...

virtual bool	create_message (int seq, const motoman_msgs::DynamicJointsGroup &pt, SimpleMessage *msg)

virtual bool	create_message_ex (int seq, const motoman_msgs::DynamicJointPoint &point, SimpleMessage *msg)

virtual bool	init (SmplMsgConnection *connection, const std::vector< std::string > &joint_names, const std::map< std::string, double > &velocity_limits=std::map< std::string, double >())
	Class initializer. More...

virtual bool	init (SmplMsgConnection *connection, const std::map< int, RobotGroup > &robot_groups, const std::map< std::string, double > &velocity_limits=std::map< std::string, double >())
	Class initializer. More...

	MotomanJointTrajectoryStreamer (int robot_id=-1)
	Default constructor. More...

virtual bool	send_to_robot (const std::vector< SimpleMessage > &messages)
	Send trajectory to robot, using this node's robot-connection. Specific method must be implemented in a derived class (e.g. streaming, download, etc.) More...

virtual void	streamingThread ()

	~MotomanJointTrajectoryStreamer ()

Public Member Functions inherited from industrial_robot_client::joint_trajectory_streamer::JointTrajectoryStreamer
virtual void	jointTrajectoryCB (const trajectory_msgs::JointTrajectoryConstPtr &msg)

virtual void	jointTrajectoryCB (const trajectory_msgs::JointTrajectoryConstPtr &msg)

virtual void	jointTrajectoryCB (const motoman_msgs::DynamicJointTrajectoryConstPtr &msg)

	JointTrajectoryStreamer (int min_buffer_size=1)

	JointTrajectoryStreamer (int min_buffer_size=1)
	Default constructor. More...

bool	send_to_robot (const std::vector< JointTrajPtMessage > &messages)

void	streamingThread ()

virtual bool	trajectory_to_msgs (const trajectory_msgs::JointTrajectoryConstPtr &traj, std::vector< JointTrajPtMessage > *msgs)

virtual bool	trajectory_to_msgs (const trajectory_msgs::JointTrajectoryConstPtr &traj, std::vector< SimpleMessage > *msgs)
	Convert ROS trajectory message into stream of SimpleMessages for sending to robot. Also includes various joint transforms that can be overridden for robot-specific behavior. More...

virtual bool	trajectory_to_msgs (const motoman_msgs::DynamicJointTrajectoryConstPtr &traj, std::vector< SimpleMessage > *msgs)
	Convert ROS trajectory message into stream of SimpleMessages for sending to robot. Also includes various joint transforms that can be overridden for robot-specific behavior. More...

	~JointTrajectoryStreamer ()

	~JointTrajectoryStreamer ()

Public Member Functions inherited from industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface
virtual bool	init (std::string default_ip="", int default_port=StandardSocketPorts::MOTION)

virtual bool	init (SmplMsgConnection *connection)

virtual bool	init (std::string default_ip="", int default_port=StandardSocketPorts::MOTION, bool version_0=false)
	Initialize robot connection using default method. More...

virtual bool	init (SmplMsgConnection *connection)
	Initialize robot connection using specified method. More...

	JointTrajectoryInterface ()

	JointTrajectoryInterface ()
	Default constructor. More...

virtual void	run ()

virtual void	run ()
	Begin processing messages and publishing topics. More...

virtual	~JointTrajectoryInterface ()

virtual	~JointTrajectoryInterface ()

Protected Member Functions
bool	disableRobotCB (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
	Disable the robot. Response is true if the state was flipped or false if the state has not changed. More...

bool	enableRobotCB (std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
	Enable the robot. Response is true if the state was flipped or false if the state has not changed. More...

bool	is_valid (const trajectory_msgs::JointTrajectory &traj)

bool	is_valid (const motoman_msgs::DynamicJointTrajectory &traj)
	Validate that trajectory command meets minimum requirements. More...

void	trajectoryStop ()

Protected Member Functions inherited from industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface
virtual bool	calc_duration (const trajectory_msgs::JointTrajectoryPoint &pt, double *rbt_duration)

virtual bool	calc_duration (const trajectory_msgs::JointTrajectoryPoint &pt, double *rbt_duration)
	Compute the expected move duration for communication to the robot. If unneeded by the robot server, set to 0 (or any value). More...

virtual bool	calc_duration (const motoman_msgs::DynamicJointsGroup &pt, double *rbt_duration)
	Compute the expected move duration for communication to the robot. If unneeded by the robot server, set to 0 (or any value). More...

virtual bool	calc_speed (const trajectory_msgs::JointTrajectoryPoint &pt, double rbt_velocity, double rbt_duration)

virtual bool	calc_velocity (const trajectory_msgs::JointTrajectoryPoint &pt, double *rbt_velocity)

virtual bool	calc_velocity (const trajectory_msgs::JointTrajectoryPoint &pt, double *rbt_velocity)
	Reduce the ROS velocity commands (per-joint velocities) to a single scalar for communication to the robot. If unneeded by the robot server, set to 0 (or any value). More...

virtual bool	calc_velocity (const motoman_msgs::DynamicJointsGroup &pt, double *rbt_velocity)
	Reduce the ROS velocity commands (per-joint velocities) to a single scalar for communication to the robot. If unneeded by the robot server, set to 0 (or any value). More...

virtual void	jointStateCB (const sensor_msgs::JointStateConstPtr &msg)

virtual void	jointStateCB (const sensor_msgs::JointStateConstPtr &msg)

virtual void	jointStateCB (const sensor_msgs::JointStateConstPtr &msg, int robot_id)

virtual void	jointTrajectoryExCB (const motoman_msgs::DynamicJointTrajectoryConstPtr &msg)
	Callback function registered to ROS topic-subscribe. Transform ROS message into SimpleMessage objects and send commands to robot. More...

virtual bool	select (const std::vector< std::string > &ros_joint_names, const trajectory_msgs::JointTrajectoryPoint &ros_pt, const std::vector< std::string > &rbt_joint_names, trajectory_msgs::JointTrajectoryPoint *rbt_pt)

virtual bool	select (const std::vector< std::string > &ros_joint_names, const motoman_msgs::DynamicJointsGroup &ros_pt, const std::vector< std::string > &rbt_joint_names, motoman_msgs::DynamicJointsGroup *rbt_pt)
	Select specific joints for sending to the robot. More...

virtual bool	select (const std::vector< std::string > &ros_joint_names, const trajectory_msgs::JointTrajectoryPoint &ros_pt, const std::vector< std::string > &rbt_joint_names, trajectory_msgs::JointTrajectoryPoint *rbt_pt)
	Select specific joints for sending to the robot. More...

virtual bool	send_to_robot (const std::vector< JointTrajPtMessage > &messages)=0

virtual bool	stopMotionCB (industrial_msgs::StopMotion::Request &req, industrial_msgs::StopMotion::Response &res)

virtual bool	stopMotionCB (industrial_msgs::StopMotion::Request &req, industrial_msgs::StopMotion::Response &res)
	Callback function registered to ROS stopMotion service Sends stop-motion command to robot. More...

virtual bool	trajectory_to_msgs (const trajectory_msgs::JointTrajectoryConstPtr &traj, std::vector< JointTrajPtMessage > *msgs)

virtual bool	transform (const trajectory_msgs::JointTrajectoryPoint &pt_in, trajectory_msgs::JointTrajectoryPoint *pt_out)

virtual bool	transform (const trajectory_msgs::JointTrajectoryPoint &pt_in, trajectory_msgs::JointTrajectoryPoint *pt_out)
	Transform joint positions before publishing. Can be overridden to implement, e.g. robot-specific joint coupling. More...

virtual bool	transform (const motoman_msgs::DynamicJointsGroup &pt_in, motoman_msgs::DynamicJointsGroup *pt_out)

Static Protected Member Functions
static bool	VectorToJointData (const std::vector< double > &vec, industrial::joint_data::JointData &joints)

Protected Attributes
ros::ServiceServer	disabler_
	Service used to disable the robot controller. When disabled, all incoming goals are ignored. More...

ros::ServiceServer	enabler_
	Service used to enable the robot controller. When disabled, all incoming goals are ignored. More...

MotomanMotionCtrl	motion_ctrl_

std::map< int, MotomanMotionCtrl >	motion_ctrl_map_

int	robot_id_

Protected Attributes inherited from industrial_robot_client::joint_trajectory_streamer::JointTrajectoryStreamer
int	current_point_

std::vector< JointTrajPtMessage >	current_traj_

std::vector< SimpleMessage >	current_traj_

int	min_buffer_size_

boost::mutex	mutex_

TransferState	state_

ros::Time	streaming_start_

boost::thread *	streaming_thread_

Protected Attributes inherited from industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface
std::vector< std::string >	all_joint_names_

SmplMsgConnection *	connection_

sensor_msgs::JointState	cur_joint_pos_

std::map< int, sensor_msgs::JointState >	cur_joint_pos_map_

double	default_duration_

double	default_joint_pos_

TcpClient	default_tcp_connection_

double	default_vel_ratio_

std::map< std::string, double >	joint_vel_limits_

ros::NodeHandle	node_

std::map< int, RobotGroup >	robot_groups_

ros::ServiceServer	srv_joint_trajectory_

ros::ServiceServer	srv_joint_trajectory_ex_

std::map< int, ros::ServiceServer >	srv_joints_

ros::ServiceServer	srv_stop_motion_

std::map< int, ros::ServiceServer >	srv_stops_

ros::Subscriber	sub_cur_pos_

std::map< int, ros::Subscriber >	sub_cur_positions_

std::map< int, ros::Subscriber >	sub_joint_trajectories_

ros::Subscriber	sub_joint_trajectory_

ros::Subscriber	sub_joint_trajectory_ex_

bool	version_0_

Additional Inherited Members
Public Types inherited from industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface
typedef std::map< int, RobotGroup >::iterator	it_type

Detailed Description

Message handler that streams joint trajectories to the robot controller. Contains FS100-specific motion control commands.

THIS CLASS IS NOT THREAD-SAFE

Definition at line 65 of file joint_trajectory_streamer.h.

Constructor & Destructor Documentation

motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer::MotomanJointTrajectoryStreamer ( int robot_id = -1 )

inlineexplicit

Default constructor.

Parameters

robot_id robot group # on this controller (for multi-group systems)

Definition at line 78 of file joint_trajectory_streamer.h.

motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer::~MotomanJointTrajectoryStreamer ( )

Definition at line 120 of file joint_trajectory_streamer.cpp.

Member Function Documentation

bool motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer::create_message	(	int	seq,
		const trajectory_msgs::JointTrajectoryPoint &	pt,
		SimpleMessage *	msg
	)

virtual

Create SimpleMessage for sending to the robot.

Parameters

[in]	seq	sequence # of this point in the overall trajectory
[in]	pt	trajectory point data
[out]	msg	message for sending to robot

Returns: true on success, false otherwise

Reimplemented from industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface.

Definition at line 168 of file joint_trajectory_streamer.cpp.

bool motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer::create_message	(	int	seq,
		const motoman_msgs::DynamicJointsGroup &	pt,
		SimpleMessage *	msg
	)

virtual

Reimplemented from industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface.

Definition at line 303 of file joint_trajectory_streamer.cpp.

bool motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer::create_message_ex	(	int	seq,
		const motoman_msgs::DynamicJointPoint &	point,
		SimpleMessage *	msg
	)

virtual

Reimplemented from industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface.

Definition at line 220 of file joint_trajectory_streamer.cpp.

bool motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer::disableRobotCB	(	std_srvs::Trigger::Request &	req,
		std_srvs::Trigger::Response &	res
	)

protected

Disable the robot. Response is true if the state was flipped or false if the state has not changed.

Definition at line 126 of file joint_trajectory_streamer.cpp.

bool motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer::enableRobotCB	(	std_srvs::Trigger::Request &	req,
		std_srvs::Trigger::Response &	res
	)

protected

Enable the robot. Response is true if the state was flipped or false if the state has not changed.

Definition at line 148 of file joint_trajectory_streamer.cpp.

bool motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer::init	(	SmplMsgConnection *	connection,
		const std::vector< std::string > &	joint_names,
		const std::map< std::string, double > &	velocity_limits = `std::map<std::string, double>()`
	)

virtual

Class initializer.

Parameters

connection	simple message connection that will be used to send commands to robot (ALREADY INITIALIZED)
joint_names	list of expected joint-names. Count and order should match data sent to robot connection. Use blank-name to insert a placeholder joint position (typ. 0.0). Joints in the incoming JointTrajectory stream that are NOT listed here will be ignored.
velocity_limits	map of maximum velocities for each joint leave empty to lookup from URDF

Returns: true on success, false otherwise (an invalid message type)

Reimplemented from industrial_robot_client::joint_trajectory_streamer::JointTrajectoryStreamer.

Definition at line 98 of file joint_trajectory_streamer.cpp.

bool motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer::init	(	SmplMsgConnection *	connection,
		const std::map< int, RobotGroup > &	robot_groups,
		const std::map< std::string, double > &	velocity_limits = `std::map<std::string, double>()`
	)

virtual

Class initializer.

Parameters

connection	simple message connection that will be used to send commands to robot (ALREADY INITIALIZED)
joint_names	list of expected joint-names. Count and order should match data sent to robot connection. Use blank-name to insert a placeholder joint position (typ. 0.0). Joints in the incoming JointTrajectory stream that are NOT listed here will be ignored.
velocity_limits	map of maximum velocities for each joint leave empty to lookup from URDF

Returns: true on success, false otherwise (an invalid message type)

Reimplemented from industrial_robot_client::joint_trajectory_streamer::JointTrajectoryStreamer.

Definition at line 69 of file joint_trajectory_streamer.cpp.

bool motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer::is_valid ( const trajectory_msgs::JointTrajectory & traj )

protectedvirtual

Reimplemented from industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface.

Definition at line 483 of file joint_trajectory_streamer.cpp.

bool motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer::is_valid ( const motoman_msgs::DynamicJointTrajectory & traj )

protectedvirtual

Validate that trajectory command meets minimum requirements.

Parameters

traj	incoming trajectory

Returns: true if trajectory is valid, false otherwise

Reimplemented from industrial_robot_client::joint_trajectory_interface::JointTrajectoryInterface.

Definition at line 511 of file joint_trajectory_streamer.cpp.

bool motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer::send_to_robot ( const std::vector< SimpleMessage > & messages )

virtual

Send trajectory to robot, using this node's robot-connection. Specific method must be implemented in a derived class (e.g. streaming, download, etc.)

Parameters

messages List of SimpleMessages to send to robot.

Returns: true on success, false otherwise

Reimplemented from industrial_robot_client::joint_trajectory_streamer::JointTrajectoryStreamer.

Definition at line 370 of file joint_trajectory_streamer.cpp.

void motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer::streamingThread ( )

virtual

Reimplemented from industrial_robot_client::joint_trajectory_streamer::JointTrajectoryStreamer.

Definition at line 380 of file joint_trajectory_streamer.cpp.

void motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer::trajectoryStop ( )

protectedvirtual

Reimplemented from industrial_robot_client::joint_trajectory_streamer::JointTrajectoryStreamer.

Definition at line 476 of file joint_trajectory_streamer.cpp.

bool motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer::VectorToJointData	(	const std::vector< double > &	vec,
		industrial::joint_data::JointData &	joints
	)

staticprotected

Definition at line 354 of file joint_trajectory_streamer.cpp.

Member Data Documentation

ros::ServiceServer motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer::disabler_

protected

Service used to disable the robot controller. When disabled, all incoming goals are ignored.

Definition at line 149 of file joint_trajectory_streamer.h.

ros::ServiceServer motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer::enabler_

protected

Service used to enable the robot controller. When disabled, all incoming goals are ignored.

Definition at line 155 of file joint_trajectory_streamer.h.

MotomanMotionCtrl motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer::motion_ctrl_

protected

Definition at line 134 of file joint_trajectory_streamer.h.

std::map<int, MotomanMotionCtrl> motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer::motion_ctrl_map_

protected

Definition at line 136 of file joint_trajectory_streamer.h.

int motoman::joint_trajectory_streamer::MotomanJointTrajectoryStreamer::robot_id_

protected

Definition at line 133 of file joint_trajectory_streamer.h.

The documentation for this class was generated from the following files:

Public Member Functions

Protected Member Functions

Static Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation