preset_reconfigure.py

Go to the documentation of this file.

#!/usr/bin/env python
# coding: utf-8
 
# Copyright 2019 RT Corporation
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
 
import roslib
import rospy
import dynamic_reconfigure.client
from std_msgs.msg import UInt8
 
class PRESET_RECONFIGURE:
    def __init__( self ):
        rospy.loginfo("Wait reconfig server...")
        self.joint_list = [
                            {"control":"/crane_x7/crane_x7_control","joint":"crane_x7_shoulder_fixed_part_pan_joint"}, \
                            {"control":"/crane_x7/crane_x7_control","joint":"crane_x7_shoulder_revolute_part_tilt_joint"}, \
                            {"control":"/crane_x7/crane_x7_control","joint":"crane_x7_upper_arm_revolute_part_twist_joint"}, \
                            {"control":"/crane_x7/crane_x7_control","joint":"crane_x7_upper_arm_revolute_part_rotate_joint"}, \
                            {"control":"/crane_x7/crane_x7_control","joint":"crane_x7_lower_arm_fixed_part_joint"}, \
                            {"control":"/crane_x7/crane_x7_control","joint":"crane_x7_lower_arm_revolute_part_joint"}, \
                            {"control":"/crane_x7/crane_x7_control","joint":"crane_x7_wrist_joint"}, \
                            {"control":"/crane_x7/crane_x7_control","joint":"crane_x7_gripper_finger_a_joint"}, \
                        ]
 
        
        self.preset_init = []
        for i in range(len(self.joint_list)):
            self.preset_init.append( { "p_gain": 800, "i_gain": 0, "d_gain": 0 } )
 
        
        self.preset_1 = [   
                            { "name":"crane_x7_shoulder_fixed_part_pan_joint",          "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_shoulder_revolute_part_tilt_joint",      "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_upper_arm_revolute_part_twist_joint",    "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_upper_arm_revolute_part_rotate_joint",   "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_lower_arm_fixed_part_joint",             "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_lower_arm_revolute_part_joint",          "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_wrist_joint",                            "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_gripper_finger_a_joint",                 "p_gain": 10, "i_gain": 0, "d_gain": 0 },\
                        ]
 
        
        self.preset_2 = [   
                            { "name":"crane_x7_shoulder_fixed_part_pan_joint",          "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_shoulder_revolute_part_tilt_joint",      "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_upper_arm_revolute_part_twist_joint",    "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_upper_arm_revolute_part_rotate_joint",   "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_lower_arm_fixed_part_joint",             "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_lower_arm_revolute_part_joint",          "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_wrist_joint",                            "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_gripper_finger_a_joint",                 "p_gain": 400, "i_gain": 0, "d_gain": 0 },\
                        ]
 
        
        self.preset_3 = [   
                            { "name":"crane_x7_shoulder_fixed_part_pan_joint",          "p_gain": 5, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_shoulder_revolute_part_tilt_joint",      "p_gain": 5, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_upper_arm_revolute_part_twist_joint",    "p_gain": 5, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_upper_arm_revolute_part_rotate_joint",   "p_gain": 5, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_lower_arm_fixed_part_joint",             "p_gain": 1, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_lower_arm_revolute_part_joint",          "p_gain": 1, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_wrist_joint",                            "p_gain": 1, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_gripper_finger_a_joint",                 "p_gain": 1, "i_gain": 0, "d_gain": 0 },\
                        ]
 
        
        self.preset_4 = [   
                            { "name":"crane_x7_shoulder_fixed_part_pan_joint",          "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_shoulder_revolute_part_tilt_joint",      "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_upper_arm_revolute_part_twist_joint",    "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_upper_arm_revolute_part_rotate_joint",   "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_lower_arm_fixed_part_joint",             "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_lower_arm_revolute_part_joint",          "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_wrist_joint",                            "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
                            { "name":"crane_x7_gripper_finger_a_joint",                 "p_gain": 800, "i_gain": 0, "d_gain": 0 },\
                        ]
 
        
        self.preset_list = []
        self.preset_list.append( self.preset_init )
        self.preset_list.append( self.preset_1 )
        self.preset_list.append( self.preset_2 )
        self.preset_list.append( self.preset_3 )
        self.preset_list.append( self.preset_4 )
 
        self.reconfigure = []
        for joint in self.joint_list:
            self.reconfigure.append( {"client":dynamic_reconfigure.client.Client( joint["control"]+"/"+joint["joint"],timeout=10 ), \
                                      "joint:":joint["joint"]} )
        rospy.loginfo("Wait sub...")
        self.subscribe = rospy.Subscriber("preset_gain_no", UInt8, self.preset_no_callback)
        rospy.loginfo("Init finished.")
 
 
    def preset_no_callback(self, no):
        joint_no = 0
        for conf in self.reconfigure:
            conf["client"].update_configuration( {"position_p_gain":self.preset_list[no.data][joint_no]["p_gain"],"position_i_gain":self.preset_list[no.data][joint_no]["i_gain"],"position_d_gain":self.preset_list[no.data][joint_no]["d_gain"]} )
            joint_no = joint_no + 1
 
 
if __name__ == '__main__':
    rospy.init_node('preset_reconfigure')
    pr = PRESET_RECONFIGURE()
    rospy.spin()