take_static_laser_collision_map.py

Go to the documentation of this file.

#! /usr/bin/env python

import roslib; roslib.load_manifest('pr2_arm_navigation_actions')
import rospy

#move arm messages and action
import actionlib
import pr2_msgs.srv
import arm_navigation_msgs.msg
import arm_navigation_msgs.msg

laser_service_name="/laser_tilt_controller/set_periodic_cmd"

def set_laser_to_slow_scan():

    global laser_service_name

    rospy.wait_for_service(laser_service_name)
    set_laser_per = rospy.ServiceProxy(laser_service_name,pr2_msgs.srv.SetPeriodicCmd)

    laser_per = pr2_msgs.srv.SetPeriodicCmdRequest()

    laser_per.command.header.stamp = rospy.Time.now()
    laser_per.command.profile = "linear"
    laser_per.command.period = 10
    laser_per.command.amplitude = .25
    laser_per.command.offset = .5
    resp1 = set_laser_per(laser_per)

    if(resp1):
        print "laser cmd successful"
    else:
        print "something wrong with laser command"


def trigger_static_map():

    static_map_client = actionlib.SimpleActionClient("make_static_collision_map", arm_navigation_msgs.msg.MakeStaticCollisionMapAction)

    static_map_client.wait_for_server()

    static_map_goal = arm_navigation_msgs.msg.MakeStaticCollisionMapGoal()
    static_map_goal.cloud_source = "full_cloud_filtered"
    static_map_goal.number_of_clouds = 2

    static_map_client.send_goal(static_map_goal)
    
    set_laser_to_slow_scan()

    static_map_client.wait_for_result()

    print 'Should have taken static map'
    
if __name__ == '__main__':
    # Initializes a rospy node so that the SimpleActionClient can
    # publish and subscribe over ROS.
    rospy.init_node('take_static_laser_collision_map')
    
    trigger_static_map()
    
    exit()