ROS Interface

Modifying the World Tree

This tutorial shows you how to add objects to the world using the update_world service offered by Giskard. These objects can have simple shapes like spheres and cubes but you can use meshes to define the shape of an object.

The following example script will add a simple sphere to the world tree. In order to do that we have to create a UpdateWorldRequest request and call the service.

import rospy
from geometry_msgs.msg import Point, Quaternion, PoseStamped
from giskard_msgs.srv import UpdateWorld, UpdateWorldRequest
from giskard_msgs.msg import WorldBody
from shape_msgs.msg import SolidPrimitive


# The get_sphere_request() function creates a WorldBody which is defined in the giskard_msgs Package.
# The WorldBody message is used to specify which object you want to add to the world.
# Then the newly created WorldBody is used to create a request for the UpdateWorld service
# which is also defined in giskard_msgs.
def get_sphere_request():
    req = UpdateWorldRequest()
    req.operation = UpdateWorldRequest.ADD

    # name of the object
    req.group_name = 'muh'

    # object geometry
    sphere = WorldBody()
    sphere.type = WorldBody.PRIMITIVE_BODY
    sphere.shape.type = SolidPrimitive.SPHERE
    sphere.shape.dimensions.append(0.05)  # radius of 5cm
    req.body = sphere

    # object pose
    pose = PoseStamped()
    pose.header.stamp = rospy.Time.now()
    pose.header.frame_id = 'map'
    pose.pose.position = Point(1.25, -0.3, 0.77)
    pose.pose.orientation = Quaternion(0, 0, 0, 1)
    req.pose = pose

    # where to attach the object
    req.parent_link = 'map'
    return req


rospy.init_node('upload_object')
# connect to service
rospy.wait_for_service('giskard/update_world')
update_world = rospy.ServiceProxy('giskard/update_world', UpdateWorld)

# create goal
request = get_sphere_request()

# send request
response = update_world(request)
print(response)

To run this script, you need Giskard running in standalone mode.

roslaunch giskardpy giskardpy_pr2_standalone.launch

Sending goals

This tutorials shows you how to send goals to the robot in python using the action interface. In general, Giskard uses a json interface. You can call any goal defined in any of the files in here. To do so, you need to set the name of the class and specify the key word arguments of its __init__ function. Here are example for how to call a joint goal and a Cartesian space goal.

import rospy
from geometry_msgs.msg import PoseStamped
from rospy_message_converter.message_converter import convert_ros_message_to_dictionary
 
from giskard_msgs.msg import MoveCmd, Constraint, CollisionEntry
from giskard_msgs.msg import MoveAction
from giskard_msgs.msg import MoveGoal
import json
import actionlib
from giskard_msgs.msg import MoveResult
 
 
def execute_joint_goal():
    # Creates the SimpleActionClient, passing the type of the action
    # (MoveAction) to the constructor.
    client = actionlib.SimpleActionClient("/giskard/command", MoveAction)
 
    # Waits until the action server has started up and started
    # listening for goals.
    print('waiting for giskard')
    client.wait_for_server()
    print('connected to giskard')
 
    # Creates a goal to send to the action server.
    action_goal = MoveGoal()
    action_goal.type = MoveGoal.PLAN_AND_EXECUTE
 
    goal = MoveCmd()
 
    constraint = Constraint()
    # Create a constraint of type JointPositionList 
    constraint.type = 'JointPositionList'
    # Create a kwargs dict as defined in the class __init__
    kwargs = {'goal_state': {'r_elbow_flex_joint': -1.29610152504,
                             'r_forearm_roll_joint': -0.0301682323805,
                             'r_shoulder_lift_joint': 1.20324921318,
                             'r_shoulder_pan_joint': -0.73456435706,
                             'r_upper_arm_roll_joint': -0.70790051778,
                             'r_wrist_flex_joint': -0.10001,
                             'r_wrist_roll_joint': 0.258268529825,
 
                             'l_elbow_flex_joint': -1.29610152504,
                             'l_forearm_roll_joint': 0.0301682323805,
                             'l_shoulder_lift_joint': 1.20324921318,
                             'l_shoulder_pan_joint': 0.73456435706,
                             'l_upper_arm_roll_joint': 0.70790051778,
                             'l_wrist_flex_joint': -0.1001,
                             'l_wrist_roll_joint': -0.258268529825,
 
                             'torso_lift_joint': 0.2,
                             'head_pan_joint': 0,
                             'head_tilt_joint': 0}
              }
    # turn kwargs into json format
    constraint.parameter_value_pair = json.dumps(kwargs)
 
    goal.constraints.append(constraint)
    action_goal.cmd_seq.append(goal)
 
    # Sends the goal to the action server.
    client.send_goal(action_goal)
 
    # Waits for the server to finish performing the action.
    client.wait_for_result()
 
    result = client.get_result()  # type: MoveResult
    if result.error_codes[0] == MoveResult.SUCCESS:
        print('giskard returned success')
    else:
        print('something went wrong')
 
 
def execute_cartesian_goal():
    # Creates the SimpleActionClient, passing the type of the action
    # (MoveAction) to the constructor.
    client = actionlib.SimpleActionClient("/giskard/command", MoveAction)
 
    # Waits until the action server has started up and started
    # listening for goals.
    print('waiting for giskard')
    client.wait_for_server()
    print('connected to giskard')
 
    # Creates a goal to send to the action server.
    action_goal = MoveGoal()
    action_goal.type = MoveGoal.PLAN_AND_EXECUTE
 
    goal = MoveCmd()
 
    constraint = Constraint()
    # Create a constraint of type CartesianPose 
    constraint.type = 'CartesianPose'
 
    goal_pose = PoseStamped()
    goal_pose.header.frame_id = 'l_gripper_tool_frame'
    goal_pose.pose.position.y = -0.1
    goal_pose.pose.position.z = 0.3
    goal_pose.pose.orientation.w = 1
 
    # Create a kwargs dict as defined in __init__ of class definition
    kwargs = {'root_link': 'map',
              'tip_link': 'l_gripper_tool_frame',
              # If you want to send ros messages, you need to specify the message type, such that Giskard knows how to
              # turn it back into a message and convert the message into a dict as well
              'goal_pose': {'message_type': 'geometry_msgs/PoseStamped',
                            'message': convert_ros_message_to_dictionary(goal_pose)}}
    # turn the kwargs into json format
    constraint.parameter_value_pair = json.dumps(kwargs)
 
    goal.constraints.append(constraint)
 
    # allow all collision
    ce = CollisionEntry()
    ce.group1 = CollisionEntry.ALL
    ce.group2 = CollisionEntry.ALL
    ce.type = CollisionEntry.ALLOW_COLLISION
    goal.collisions.append(ce)
 
    action_goal.cmd_seq.append(goal)
 
    # Sends the goal to the action server.
    client.send_goal(action_goal)
 
    # Waits for the server to finish performing the action.
    client.wait_for_result()
 
    result = client.get_result()  # type: MoveResult
    if result.error_codes[0] == MoveResult.SUCCESS:
        print('giskard returned success')
    else:
        print('something went wrong')
 
 
if __name__ == '__main__':
    try:
        # Initializes a rospy node so that the SimpleActionClient can
        # publish and subscribe over ROS.
        rospy.init_node('joint_space_client')
        execute_joint_goal()
        execute_cartesian_goal()
    except rospy.ROSInterruptException:
        print("program interrupted before completion")

To run this script, you need Giskard running in standalone mode.

roslaunch giskardpy giskardpy_pr2_standalone.launch