From fe3a7e9fe7c823e7530ffad8fb9c1ef4ac979671 Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Thu, 29 Oct 2015 19:23:42 +0100 Subject: [PATCH] added dynamic reconfigure --- CMakeLists.txt | 5 +++++ config/arm_ros_conn.cfg | 14 ++++++++++++++ launch/arm.launch | 2 +- package.xml | 2 ++ scripts/{arm_ros_conn.py => arm_ros.py} | 21 ++++++++++++++------- 5 files changed, 36 insertions(+), 8 deletions(-) create mode 100755 config/arm_ros_conn.cfg rename scripts/{arm_ros_conn.py => arm_ros.py} (85%) diff --git a/CMakeLists.txt b/CMakeLists.txt index c64d68d..c286355 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(catkin REQUIRED COMPONENTS rospy sensor_msgs std_msgs + dynamic_reconfigure ) ## System dependencies are found with CMake's conventions @@ -71,6 +72,10 @@ find_package(catkin REQUIRED COMPONENTS # geometry_msgs# sensor_msgs# std_msgs # ) +generate_dynamic_reconfigure_options( + config/arm_ros_conn.cfg +) + ################################### ## catkin specific configuration ## ################################### diff --git a/config/arm_ros_conn.cfg b/config/arm_ros_conn.cfg new file mode 100755 index 0000000..38f46d3 --- /dev/null +++ b/config/arm_ros_conn.cfg @@ -0,0 +1,14 @@ +#!/usr/bin/env python + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("speed_1", int_t, 0, "PWM arm_base_to_link1", 220, 0, 255) +gen.add("speed_2", int_t, 0, "PWM link_1_2_joint", 220, 0, 255) +gen.add("speed_3", int_t, 0, "PWM link_2_3_joint", 220, 0, 255) +gen.add("speed_4", int_t, 0, "PWM link_3_4_joint", 220, 0, 255) +gen.add("speed_5", int_t, 0, "PWM link_4_5_joint", 220, 0, 255) +gen.add("speed_6", int_t, 0, "PWM gripper", 220, 0, 255) + +exit(gen.generate("arm_ros_conn", "arm_ros_conn", "Arm")) diff --git a/launch/arm.launch b/launch/arm.launch index 46cc15e..7369dac 100644 --- a/launch/arm.launch +++ b/launch/arm.launch @@ -4,5 +4,5 @@ - + diff --git a/package.xml b/package.xml index bf41ffb..9ad1151 100644 --- a/package.xml +++ b/package.xml @@ -44,10 +44,12 @@ rospy sensor_msgs std_msgs + dynamic_reconfigure geometry_msgs rospy sensor_msgs std_msgs + dynamic_reconfigure diff --git a/scripts/arm_ros_conn.py b/scripts/arm_ros.py similarity index 85% rename from scripts/arm_ros_conn.py rename to scripts/arm_ros.py index 4011356..818b3c1 100755 --- a/scripts/arm_ros_conn.py +++ b/scripts/arm_ros.py @@ -6,6 +6,8 @@ import rospy import arm import actionlib import numpy as np +from dynamic_reconfigure.server import Server +from arm_ros_conn.cfg import ArmConfig from sensor_msgs.msg import JointState from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryActionResult, FollowJointTrajectoryActionFeedback, FollowJointTrajectoryResult from control_msgs.msg import GripperCommandAction, GripperCommandActionResult, GripperCommandFeedback @@ -25,7 +27,7 @@ class ARMRosConn(): def __init__(self): rospy.init_node('arm') - self.speed = 220 + self.lSpeeds = [220] * 6 self.lAngles = [0] * 6 arm.switch(0) arm.switch(2) @@ -39,6 +41,7 @@ class ARMRosConn(): self._as_gripper = actionlib.SimpleActionServer("gripper_controller/gripper_action", GripperCommandAction, execute_cb=self.execute_gripper_action, auto_start = False) self._as_gripper.start() self.pub_joint_states = rospy.Publisher("joint_states", JointState, queue_size=16) + self.dyn_conf = Server(ArmConfig, self.execute_dyn_reconf) self.run() def run(self): @@ -47,6 +50,10 @@ class ARMRosConn(): self.publish_joint_states() rate.sleep() + def execute_dyn_reconf(self, config, level): + self.lSpeeds = [config["speed_1"], config["speed_2"], config["speed_3"], config["speed_4"], config["speed_5"], config["speed_6"]] + return config + def publish_joint_states(self): joint_state = JointState() joint_state.header.stamp = rospy.Time.now() @@ -68,11 +75,11 @@ class ARMRosConn(): point.positions[goal.trajectory.joint_names.index(lJointNames[4])], ] try: - arm.to_angle(0, self.speed, -lGoalPosOrdered[0]) - arm.to_angle(1, self.speed, lGoalPosOrdered[1]) - arm.to_angle(2, self.speed, -lGoalPosOrdered[2]) - arm.to_angle(3, self.speed, -lGoalPosOrdered[3]) - arm.to_angle(4, self.speed, lGoalPosOrdered[4]) + arm.to_angle(0, self.lSpeeds[0], -lGoalPosOrdered[0]) + arm.to_angle(1, self.lSpeeds[1], lGoalPosOrdered[1]) + arm.to_angle(2, self.lSpeeds[2], -lGoalPosOrdered[2]) + arm.to_angle(3, self.lSpeeds[3], -lGoalPosOrdered[3]) + arm.to_angle(4, self.lSpeeds[4], lGoalPosOrdered[4]) except arm.RangeError as e: print >> sys.stderr, e.message self._feedback.status = GoalStatus.REJECTED @@ -103,7 +110,7 @@ class ARMRosConn(): def execute_gripper_action(self, goal): - arm.to_angle(5, self.speed, 0.35-goal.command.position) + arm.to_angle(5, self.lSpeeds[5], 0.35-goal.command.position) while True: error = goal.command.position - self.lAngles[-1] if abs(error) < 0.02: -- 2.39.2