Modules in irsl_choreonoid_ros

https://github.com/IRSL-tut/irsl_choreonoid_ros

irsl_choreonoid_ros.cnoid_ros_util

https://github.com/IRSL-tut/irsl_choreonoid_ros/blob/main/irsl_choreonoid_ros/cnoid_ros_util.py

irsl_choreonoid_ros.cnoid_ros_util.parseURLROS(url)

Refere parseURL in irsl_choreonoid.cnoid_util

Parameters:: url (str) – url
Returns:: Absolute path
Return type:: str

Examples

>>> parseURLROS('package://ros_package_name/dir/file')
/catkin_ws/install/share/ros_package_name/dir/file

irsl_choreonoid_ros.setup_cnoid

https://github.com/IRSL-tut/irsl_choreonoid_ros/blob/main/irsl_choreonoid_ros/setup_cnoid.py

class irsl_choreonoid_ros.setup_cnoid.SetupCnoid(rootItem=None, worldItem=None)

Bases: object

Utility class for setting .cnoid file from python script

__init__(rootItem=None, worldItem=None)

Parameters:

rootItem (cnoid.Base.Item, optional) – If set, it is used as root for creating environment
worldItem (cnoid.Base.WorldItem, optional) – If set, it is used as a worldItem

buildEnvironment(info_dict, world='World', createWorld=False, setCamera=False, offset=None)

Building environment (setting objects) under the WorldItem

Parameters:

dict['key' (info_dict () – value] ) : Dictionary for representing objects on environment
world (str, default='World') – Name of WorldItem, added objects under this item
craeteWorld (boolean, default=False) – If True, creating new WorldItem
setCamera (boolean, default=False) – If True, set camera position
offset (cnoid.IRSLCoords.coordinates) – Offset of objects

createCnoid(info_dict, addDefaultSimulator=True, addDefaultWorld=True, noEnvironment=False)

Creating project from parameters

Parameters:

dict['key' (info_dict () – value] ) : Dictionary for representing the project
addDefaultSimulator (boolean, default=True) – If True, adding new SimulatorItem if there is no instruction in info_dict
addDefaultWorld (boolean, default=True) – If True, adding new WorldItem if there is no instruction in info_dict
noEnvironment (boolean, default=False) – If True, not adding environment(objects). Use buildEnvironment method.

buildEnvironmentFromYaml(yamlFile, **kwargs)

Building environment from yaml-file

Parameters:

yamlFile (str) – File name to load
kwargs (dict) – Keyword to pass to setup_cnoid.buildEnvironment

createCnoidFromYaml(yamlFile, **kwargs)

Creating project from yaml-file

Parameters:

yamlFile (str) – File name to load
kwargs (dict) – Keyword to pass to setup_cnoid.createCnoid

classmethod setEnvironmentFromYaml(yamlFile, **kwargs)

Setup environment from yaml-file (classmethod of buildEnvironmentFromYaml)

Parameters:

yamlFile (str) – File name to load
kwargs (dict) – Keyword to pass to setup_cnoid.buildEnvironment

Returns:

Instance of setup_cnoid

Return type:

irsl_choreonoid_ros.setup_cnoid

classmethod setCnoidFromYaml(yamlFile, **kwargs)

Setup project from yaml-file (classmethod of createCnoidFromYaml)

Parameters:

yamlFile (str) – File name to load
kwargs (dict) – Keyword to pass to setup_cnoid.createCnoid

Returns:

Instance of setup_cnoid

Return type:

irsl_choreonoid_ros.setup_cnoid

classmethod setupEnvironment(info, **kwargs)

Building environment (setting objects) under the WorldItem (class method)

Parameters:

dict['key' (info_dict () – value] ) : Dictionary for representing objects on environment
world (str, default='World') – Name of WorldItem, added objects under this item
craeteWorld (boolean, default=False) – If True, creating new WorldItem
setCamera (boolean, default=False) – If True, set camera position
offset (cnoid.IRSLCoords.coordinates) – Offset of objects

classmethod setupCnoid(info, **kwargs)

Creating project from parameters (class method)

Parameters:

dict['key' (info_dict () – value] ) : Dictionary for representing the project
addDefaultSimulator (boolean, default=True) – If True, adding new SimulatorItem if there is no instruction in info_dict
addDefaultWorld (boolean, default=True) – If True, adding new WorldItem if there is no instruction in info_dict
noEnvironment (boolean, default=False) – If True, not adding environment(objects). Use buildEnvironment method.

startSimulator(realTime=None)

Starting simulation

Parameters:: realTime (boolean, default=None) – If True, simulator will run with realtime sync mode

irsl_choreonoid_ros.RobotInterface

https://github.com/IRSL-tut/irsl_choreonoid_ros/blob/main/irsl_choreonoid_ros/RobotInterface.py

class irsl_choreonoid_ros.RobotInterface.MobileBaseInterface(info, robot=None, **kwargs)

Bases: object

Interface for controlling locomotion of the robot

__init__(info, robot=None, **kwargs)

property mobile_initialized

Initialized check of MobileBase

Returns:: True returns, if MobileBase instance has been initialized
Return type:: boolean

property mobile_connected

Connection check of MobileBase

Returns:: True returns, if MobileBase instance has been connected
Return type:: boolean

waitFinishMoving(timeout=1.0)

Wait to finish moving of MobileBase

Parameters:: timeout (float, default=1.0) –
Returns:: False returns, if timeout.
Return type:: boolean

stop()

Stop moving of MobileBase

Parameters:: None –

move_velocity(vel_x, vel_y, vel_th)

Set moving velocity of MobileBase

Parameters:

vel_x (float) – Velocity of x-axis [ m/sec ]
vel_y (float) – Velocity of y-axis [ m/sec ]
vel_th (float) – Angular velocity of yaw [ radian/sec ]

property currentCoordsOnMap

Current robot’s coordinate on the map frame

Returns:: Current robot’s coordinate on the map
Return type:: cnoid.IRSLCoords.coordinates

property currentCoordsOnOdom

Current robot’s coordinate on the odom frame

Returns:: Current robot’s coordinate on the odom
Return type:: cnoid.IRSLCoords.coordinates

coordsOnMap(time=None)

Current robot’s coordinate on the map frame

Parameters:: time (rospy.Time, optional) – Time using to solve TF
Returns:: Current robot’s coordinate on the map
Return type:: cnoid.IRSLCoords.coordinates

coordsOnOdom(time=None)

Current robot’s coordinate on the odom frame

Parameters:: time (rospy.Time, optional) – Time using to solve TF
Returns:: Current robot’s coordinate on the odom
Return type:: cnoid.IRSLCoords.coordinates

move_position(coords, wrt=<wrt.local: 1>)

Set target position for MobileBase, target is reletive to current robot’s coordinates

Parameters:: coords (cnoid.IRSLCoords.coordinates) – Target coordinates for moving (reletive to current robot’s coordinates)

move_on_map(coords, relative=False, time=None, wrt=<wrt.local: 1>)

Set target position on map for MobileBase, target is relative to map coordinates

Parameters:

coords (cnoid.IRSLCoords.coordinates) – Target coordinates for moving (map coordinates)
relative (boolean, default = False) – If True, trajectory is considered it is relative to robot’s coordinates.
time (rospy.Time, optional) – If relative is True, use this time to solve tf
() (wrt) –

wait_move_on_map(timeout=None)

Wait until move_on_map method finished

Parameters:: timeout (float, optional) – If set, maximum waiting time is set

move_trajectory_map(trajectory, relative=False, time=None, wrt=<wrt.local: 1>)

Set target trajectory (based on tf of map) for MobileBase (odometry relative move)

Parameters:

trajectory (list[(cnoid.IRSLCoords.coordinates, float)]) – List of pair of coordinates and time
relative (boolean, default = False) – If True, trajectory is considered it is relative to robot’s coordinates.
time (rospy.Time, optional) – If relative is True, use this time to solve tf
() (wrt) –

move_trajectory(trajectory, relative=False, time=None, wrt=<wrt.local: 1>)

Set target trajectory for MobileBase (odometry relative move)

Parameters:

trajectory (list[(cnoid.IRSLCoords.coordinates, float)]) – List of pair of coordinates and time
relative (boolean, default = False) – If True, trajectory is considered it is relative to robot’s coordinates.
time (rospy.Time, optional) – If relative is True, use this time to solve tf
() (wrt) –

wait_move_trajectory(timeout=None)

Wait until move_trajectory method finished

Parameters:: timeout (float, optional) – If set, maximum waiting time is set

class irsl_choreonoid_ros.RobotInterface.JointInterface(info, robot=None, **kwargs)

Bases: object

Interface for controlling joints of the robot

__init__(info, robot=None, **kwargs)

property joint_initialized

Initialized check of JointInterface

Returns:: True returns, if JointInterface instance has been initialized
Return type:: boolean

property joint_connected

Connection check of JointInterface

Returns:: True returns, if JointInterface instance has been connected
Return type:: boolean

property jointGroupList

Getting list of instance of joint-group

Returns:: List of instance of joint-groups
Return type:: list [ JointGroup ]

property jointGroupNames

Getting list of name of joint-groups

Returns:: List of name of joint-groups
Return type:: list [ str ]

getJointGroup(name)

Getting a instance of joint-group

Parameters:: name (str) – Name of joint-group
Returns:: Instance of joint-group
Return type:: JointGroupTopic

sendAngles(tm=None, group=None)

Sending angles of self.robot to the actual robot

Parameters:

tm (float) – Moving duration in second
group (str or list[str], optional) – Name(s) of group to be used

sendAngleVector(angle_vector, tm=None, group=None, wait=False, waitTimeout=None)

Sending angle-vector to the actual robot. angle_vector is set to self.robot

Parameters:

angle_vector (numpy.array) – Vector of angles of whole body
tm (float) – Moving duration in second
group (str or list[str], optional) – Name(s) of group to be used
wait (boolean, defaut=False) – Wait until finishing moveving
waitTimeout (float, optional) – Pass to waitUntilFinish

sendAngleVectorSequence(angle_vector_list, tm_list, group=None, wait=False, waitTimeout=None)

Parameters:

angle_vector_list (list [numpy.array]) – List of vectors of angles
tm_list (list[ float ]) – List of moving durations in second
group (str or list[str], optional) – Name(s) of group to be used
wait (bolean, default=False) – wait until finishing move
waitTimeout (float, optional) – Pass to waitUntilFinish

sendAngleMap(angle_map, tm, group=None, wait=False, waitTimeout=None)

Sending angles to the actual robot. angles is set to self.robot

Parameters:

angle_map (dict[name, float]) – Dictionary, whose key is joint-name, and value is joint-angle
tm (float) – Moving duration in second
group (str or list[str], optional) – Name(s) of group to be used

isFinished(group=None)

Checking method for finishing to send angles

Parameters:: group (str or list[str], optional) – Name(s) of group to be used
Returns:: If True, the robot is not moving
Return type:: boolean

waitUntilFinish(timeout=None, group=None)

Waiting until finishing joint moving

Parameters:

timeout (float, optional) – Time for timeout
group (str or list[str], optional) – Name(s) of group to be used

Returns:

False returns, if timeout.

Return type:

boolean

class irsl_choreonoid_ros.RobotInterface.DeviceInterface(info, robot=None, **kwargs)

Bases: object

Interface for receiving data from sensors on the robot

__init__(info, robot=None, **kwargs)

property device_initialized

Initialized check of DeviceInterface

Returns:: True returns, if DeviceInterface instance has been initialized
Return type:: boolean

property device_connected

Connection check of DeviceInterface

Returns:: True returns, if all devices in this instance have been connected
Return type:: boolean

property deviceList

Getting list of the instances for gathering data from the device

Returns:: List of the instances for gathering data
Return type:: list [ RosDeviceBase ]

property deviceNames

Getting list of name of the devices

Returns:: List of name of the devices
Return type:: list [ str ]

getDevice(name)

Getting the instance for gathering data from the device

Parameters:: name (str) – Name of the device
Returns:: The instance witch has the same name as given name
Return type:: RosDeviceBase

getDevicesByClass(cls)

Getting the list of device which is a typical class

Parameters:: cls (Class) – Class of the device
Returns:: List of the instance whose class is subclass of cls
Return type:: list [obj]

data(name, clear=False)

Getting data from the device

Parameters:

name (str) – Name of the device
clear (boolean, default = False) – Clear current-data

Returns:

Data from the device. Type of a return value depends on type of the devide.

Return type:

ANY

waitData(name, timeout=None, clear=False)

Getting data, waiting if there is no current data

Parameters:

name (str) – Name of the device
timeout (float, optional) – Time out in second
clear (boolean, default = False) – Clear current-data

Returns:

Data from the device. Type of a return value depends on type of the devide.

Return type:

ANY

waitNextData(name, timeout=None, clear=False)

Getting data, waiting until subscribing new data

Parameters:

name (str) – Name of the device
timeout (float, optional) – Time out in second
clear (boolean, default = False) – Clear current-data

Returns:

Data from the device. Type of a return value depends on type of the devide.

Return type:

ANY

dataArray(names, clear=False)

Getting list of data from devices

Parameters:

names (list[str]) – Name of the device
clear (boolean, default = False) – Clear current-data

Returns:

Data from the device. Type of a return value depends on type of the devide.

Return type:

list[ANY]

waitDataArray(names, timeout=None, clear=False)

Getting list of data, waiting if there is no current data

Parameters:

names (list[str]) – Name of the device
timeout (float, optional) – Time out in second
clear (boolean, default = False) – Clear current-data

Returns:

Data from the device. Type of a return value depends on type of the devide.

Return type:

ANY

waitNextDataArray(names, timeout=None, clear=False)

Getting list of data, waiting until subscribing new data

Parameters:

names (list[str]) – Name of the device
timeout (float, optional) – Time out in second
clear (boolean, default = False) – Clear current-data

Returns:

Data from the device. Type of a return value depends on type of the devide.

Return type:

ANY

addDevice(dev, force=False)

Adding device to

Parameters:

dev (RosDeviceBase) – Instance of device
force (boolean, default=False) – If True, overwrite when duplicate

Returns: boolean : Returns True if device was added successfully

removeDevice(devname, purge=True)

Removing device from

Parameters:

devname (str) – Name of device
purge (boolean, default=True) – If True, destruct device object

Returns:

Returns True if device was removed successfully

Return type:

boolean

class irsl_choreonoid_ros.RobotInterface.OneShotSubscriber(topic, msg, size=1)

Bases: RosDeviceBase

Subscribe just N messages

__init__(topic, msg, size=1): ” :param topic: Name of Topic :type topic: str :param msg: Class instance of message to subscribe :type msg: class :param size: Size of messages to be subscribed :type size: int, default = 1

waitResults(timeout=None)

“Waiting N results

Parameters:

topic (str) – Name of Topic
msg (class) – Class instance of message to subscribe
size (int, default = 1) – Size of messages to be subscribed

Returns:

Subscribed messages. If timeout occurs, None is returned.

Return type:

list( ros_message )

class irsl_choreonoid_ros.RobotInterface.OneShotSyncSubscriber(topics, msgs, size=1, sync_message_queue_size=100, sync_slop=0.02)

Bases: RosDeviceBase

Subscribe just N sync messages

__init__(topics, msgs, size=1, sync_message_queue_size=100, sync_slop=0.02): ” :param topics: Name of Topic :type topics: list of str :param msgs: Class instance of message to subscribe :type msgs: list of class :param size: Size of messages to be subscribed :type size: int, default = 1 :param sync_message_queue_size: Size of message queue :type sync_message_queue_size: int, default = 100 :param sync_slop: Delay with which messages can be synchronized in seconds). :type sync_slop: float, default = 0.02

waitResults(timeout=None)

“Waiting N results

Parameters:: timeout (float, default = None) – Time for timeout
Returns:: None

class irsl_choreonoid_ros.RobotInterface.RobotInterface(file_name, node_name='robot_interface', anonymous=False, connection_wait=3.0, connection=True, MASTER_URI=None, MASTER=None, MASTER_PORT=11311, IP=None, HOSTNAME=None)

Bases: JointInterface, DeviceInterface, MobileBaseInterface

Interface for controllring robot (inheriting classes JointInterface, DeviceInterface and MobileBaseInterface)

At a instance of this interface, all methods in JointInterface, DeviceInterface and MobileBaseInterface can be used.

Then, please refer methods of these classes.

__init__(file_name, node_name='robot_interface', anonymous=False, connection_wait=3.0, connection=True, MASTER_URI=None, MASTER=None, MASTER_PORT=11311, IP=None, HOSTNAME=None)

Parameters:

file_name (str) – Name of setting.yaml file
node_name (str, default='robot_interface') – Name of node
anonymous (boolean, default = False) – If True, ROS node will start with this node-name.
connection_wait (float, default=3.0) – Wait until ROS connection has established
connection (boolean, default=True) – If false, create instace without ROS connection
MASTER_URI (str, optional) – Set environment variable, ROS_MASTER_URI if MASTER is not set
MASTER (str, optional) – Set environment variable, ROS_MASTER_URI = http://MASTER:MASTER_PORT
MASTER_PORT (int, default 11311) – Set environment variable, ROS_MASTER_URI = http://MASTER:MASTER_PORT
IP (str, optional) – Set environment variable, ROS_IP
HOSTNAME (str, optional) – Set environment variable, ROS_HOSTNAME

getRobotModel(asItem=True)

Return an instance of RobotModel (irsl_choreonoid.robot_util.RobotModelWrapped)

Parameters:: asItem (boolean, default=True) – If True, model is generated as cnoid.BodyPlugin.BodyItem
Returns:: Newly generated instance
Return type:: irsl_choreonoid.robot_util.RobotModelWrapped

getActualRobotModel(asItem=True)

Return an instance of RobotModel (irsl_choreonoid.robot_util.RobotModelWrapped) with RobotInterface internal robot-model

Parameters:: asItem (boolean, default=True) – If True, model is generated as cnoid.BodyPlugin.BodyItem
Returns:: RobotModel created from RobotInterface internal robot-model
Return type:: irsl_choreonoid.robot_util.RobotModelWrapped

copyRobot()

Return other instance of the robot model

Parameters:: None –
Returns:: Copy of self.robot (this is not identical to self.robot)
Return type:: cnoid.Body.Body

isRealRobot()

Checking this interface working with a real robot

Parameters:: None –

Retuns:: boolean : If True, this interface connected to a real robot, otherwise connected to simulator

property effortVector

Return vector of effort of actual robot (sensing value)

This method requires JointState class in devices

Returns:: 1 x N vector ( N is len(jointList) )
Return type:: numpy.array

property velocityVector

Return vector of velocity of actual robot (sensing value)

This method requires JointState class in devices

Returns:: 1 x N vector ( N is len(jointList) )
Return type:: numpy.array

property actualAngleVector

Return angle-vector of actual robot (sensing value)

This method requires JointState class in devices

Returns:: 1 x N vector ( N is len(jointList) )
Return type:: numpy.array

property referenceAngleVector

Return reference angle-vector (value of past command)

This method requires JointTrajectoryState class in devices

Returns:: 1 x N vector ( N is len(jointList) )
Return type:: numpy.array

property robot

Return instance of the robot model (applying sensor values)

Returns:: Instance of the robot model using in this instance
Return type:: cnoid.Body.Body

property jointRobot

Return instance of the robot model (using for sending command)

Returns:: Instance of the robot model using in this instance
Return type:: cnoid.Body.Body

oneShotSubscriber(topic, msg, size=1)

Return instance of OneShotSubscriber

Parameters:

topic (str) – Name of Topic
msg (class) – Class instance of message to subscribe
size (int, default = 1) – Size of messages to be subscribed

Returns:

Instance of OneShotSubscriber

Return type:

OneShotSubscriber

Examples

one = ri.oneShotSubscriber(‘topicname’, sensor_msgs.msg.Image) res = one.waitResults(5)

oneShotSyncSubscriber(topics, msgs, size=1, sync_message_queue_size=100, sync_slop=0.02)

” :param topic: Name of Topic :type topic: list of str :param msg: Class instance of message to subscribe :type msg: list of class :param size: Size of messages to be subscribed :type size: int, default = 1 :param sync_message_queue_size: Size of message queue :type sync_message_queue_size: int, default = 100 :param sync_slop: Delay with which messages can be synchronized in seconds) :type sync_slop: float, default = 0.02

Returns:: Instance of OneShotSyncSubscriber
Return type:: OneShotSyncSubscriber

Examples

one = ri.oneShotSyncSubscriber([‘topicname1’, ‘topicname2’], [sensor_msgs.msg.Image, sensor_msgs.msg.CameraInfo]) res = one.waitResults(5)

getRosDevice(topic, msg, name=None)

Parameters:

topic (str) – Name of Topic
msg (class) – Class instance of message to subscribe
name (str, default = 'rosdevice') – Name of device

Returns:

Instance of RosDevice

Return type:

RosDevice

Examples

dev = ri.getRosDeivce(‘topicname’, sensor_msgs.msg.Image, name=’Camera0’) res = dev.getNextData(5)