Merge lp:~shadowrobot/sr-ros-interface-ethercat/Bug_change_control_type into lp:sr-ros-interface-ethercat

Proposed by Toni Oliver on 2012-10-30
Status: Merged
Merged at revision: 562
Proposed branch: lp:~shadowrobot/sr-ros-interface-ethercat/Bug_change_control_type
Merge into: lp:sr-ros-interface-ethercat
Diff against target: 244 lines (+121/-17)
6 files modified
.bzrignore (+1/-0)
sr_edc_controller_configuration/sr_edc_default_controllers.launch (+0/-11)
sr_edc_launch/sr_edc.launch (+1/-1)
sr_robot_lib/include/sr_robot_lib/sr_robot_lib.hpp (+22/-0)
sr_robot_lib/manifest.xml (+1/-0)
sr_robot_lib/src/sr_robot_lib.cpp (+96/-5)
To merge this branch: bzr merge lp:~shadowrobot/sr-ros-interface-ethercat/Bug_change_control_type
Reviewer Review Type Date Requested Status
Ugo 2012-10-30 Approve on 2012-10-31
Review via email: mp+132132@code.launchpad.net

Description of the change

Added a mechanism to deal with the changes from PWM to torque. When the change_control_type service is called, the correct parameters for each joint are loaded in the Param Server, and the resetGains service for every controller loaded is invoked.

To post a comment you must log in.
Ugo (ugocupcic) :
review: Approve

Preview Diff

[H/L] Next/Prev Comment, [J/K] Next/Prev File, [N/P] Next/Prev Hunk
1=== modified file '.bzrignore'
2--- .bzrignore 2012-03-22 15:46:10 +0000
3+++ .bzrignore 2012-10-30 15:09:22 +0000
4@@ -29,3 +29,4 @@
5 sr_edc_launch/build
6 sr_edc_controller_configuration/bin
7 sr_edc_controller_configuration/build
8+sr_robot_lib/_gtest_from_src
9
10=== removed file 'sr_edc_controller_configuration/sr_edc_default_controllers.launch'
11--- sr_edc_controller_configuration/sr_edc_default_controllers.launch 2012-03-23 18:17:00 +0000
12+++ sr_edc_controller_configuration/sr_edc_default_controllers.launch 1970-01-01 00:00:00 +0000
13@@ -1,11 +0,0 @@
14-<launch>
15- <!-- <rosparam command="load" file="$(find sr_ethercat_hand_config)/controls/motors/motor_board_effort_controllers_stopped.yaml" /> -->
16- <!-- <rosparam command="load" file="$(find sr_ethercat_hand_config)/controls/motors/motor_board_effort_controllers_ff_and_th.yaml" /> -->
17- <rosparam command="load" file="$(find sr_ethercat_hand_config)/controls/motors/motor_board_effort_controllers.yaml" />
18- <rosparam command="load" file="$(find sr_ethercat_hand_config)/controls/friction_compensation.yaml" />
19- <rosparam command="load" file="$(find sr_ethercat_hand_config)/controls/host/sr_edc_effort_controllers.yaml" />
20- <rosparam command="load" file="$(find sr_ethercat_hand_config)/controls/host/sr_edc_joint_position_controllers.yaml" />
21- <!-- <rosparam command="load" file="$(find sr_ethercat_hand_config)/controls/host/sr_edc_joint_position_controllers_PWM.yaml" /> -->
22- <rosparam command="load" file="$(find sr_ethercat_hand_config)/controls/host/sr_edc_mixed_position_velocity_joint_controllers.yaml" />
23- <rosparam command="load" file="$(find sr_ethercat_hand_config)/controls/host/sr_edc_joint_velocity_controllers.yaml" />
24-</launch>
25
26=== modified file 'sr_edc_launch/sr_edc.launch'
27--- sr_edc_launch/sr_edc.launch 2012-08-23 12:43:40 +0000
28+++ sr_edc_launch/sr_edc.launch 2012-10-30 15:09:22 +0000
29@@ -7,7 +7,7 @@
30 <rosparam command="load" file="$(find sr_ethercat_hand_config)/rates/motor_data_polling.yaml" />
31
32 <!-- Default controllers -->
33- <include file="$(find sr_edc_controller_configuration)/sr_edc_default_controllers.launch" />
34+ <include file="$(find sr_ethercat_hand_config)/controls/sr_edc_default_controllers.launch" />
35
36 <!-- Should be loaded before starting/running pr2_etherCAT node -->
37 <rosparam command="load" ns="srh" file="$(find sr_ethercat_hand_config)/calibrations/sr_edc_sensor_gain_offset.yaml" />
38
39=== modified file 'sr_robot_lib/include/sr_robot_lib/sr_robot_lib.hpp'
40--- sr_robot_lib/include/sr_robot_lib/sr_robot_lib.hpp 2012-03-15 10:57:58 +0000
41+++ sr_robot_lib/include/sr_robot_lib/sr_robot_lib.hpp 2012-10-30 15:09:22 +0000
42@@ -317,8 +317,19 @@
43
44 ///The current type of control (FORCE demand or PWM demand sent to the motors)
45 sr_robot_msgs::ControlType control_type_;
46+ /**
47+ * Flag to signal that there has been a change in the value of control_type_ and certain actions are required.
48+ * The flag is set in the callback function of the change_control_type_ service.
49+ * The flag is checked in build_motor_command() and the necessary actions are taken there.
50+ * These actions involve calling services in the controller manager and all the active controllers. This is the
51+ * reason why we don't do it directly in the callback function. As we use a single thread to serve the callbacks,
52+ * doing so would cause a deadlock, thus we do it in the realtime loop thread instead.
53+ */
54+ bool control_type_changed_flag_;
55 ///A service server used to change the control type on the fly.
56 ros::ServiceServer change_control_type_;
57+ ///A mutual exclusion object to ensure that no command will be sent to the robot while a change in the control type (PWM or torque) is ongoing
58+ boost::shared_ptr<boost::mutex> lock_command_sending_;
59
60 /**
61 * The callback to the change_control_type_ service. Updates
62@@ -332,6 +343,17 @@
63 bool change_control_type_callback_( sr_robot_msgs::ChangeControlType::Request& request,
64 sr_robot_msgs::ChangeControlType::Response& response );
65
66+ /**
67+ * Load the necessary parameters in the Parameter Server and
68+ * calls a service for every controller currently loaded in the controller manager to make it
69+ * reload (resetGains()) its parameters from the Parameter Server
70+ *
71+ * @param control_type The new active control type (PWM or torque)
72+ *
73+ * @return true if all the steps successful
74+ */
75+ bool change_control_parameters(int16_t control_type);
76+
77 ///The Flag which will be sent to change the motor controls
78 std::queue<std::vector<sr_robot_msgs::MotorSystemControls>, std::list<std::vector<sr_robot_msgs::MotorSystemControls> > > motor_system_control_flags_;
79 ///A service server used to call the different motor system controls "buttons"
80
81=== modified file 'sr_robot_lib/manifest.xml'
82--- sr_robot_lib/manifest.xml 2012-05-31 09:07:13 +0000
83+++ sr_robot_lib/manifest.xml 2012-10-30 15:09:22 +0000
84@@ -18,6 +18,7 @@
85 <depend package="std_srvs"/>
86 <depend package="diagnostic_updater"/>
87 <depend package="realtime_tools"/>
88+ <depend package="pr2_mechanism_msgs"/>
89
90 <depend package="roscpp"/>
91 <depend package="rospy"/>
92
93=== modified file 'sr_robot_lib/src/sr_robot_lib.cpp'
94--- sr_robot_lib/src/sr_robot_lib.cpp 2012-08-23 08:47:46 +0000
95+++ sr_robot_lib/src/sr_robot_lib.cpp 2012-10-30 15:09:22 +0000
96@@ -34,6 +34,7 @@
97
98 #include "sr_robot_lib/shadow_PSTs.hpp"
99 #include "sr_robot_lib/biotac.hpp"
100+#include <pr2_mechanism_msgs/ListControllers.h>
101
102 namespace shadow_robot
103 {
104@@ -47,8 +48,11 @@
105 : main_pic_idle_time(0), main_pic_idle_time_min(1000), nullify_demand_(false), motor_current_state(
106 operation_mode::device_update_state::INITIALIZATION), tactile_current_state(operation_mode::device_update_state::INITIALIZATION),
107 config_index(MOTOR_CONFIG_FIRST_VALUE),
108+ control_type_changed_flag_(false),
109 nh_tilde("~")
110 {
111+ lock_command_sending_ = boost::shared_ptr<boost::mutex>(new boost::mutex());
112+
113 //advertise the service to nullify the demand sent to the motor
114 // this makes it possible to easily stop the controllers.
115 nullify_demand_server_ = nh_tilde.advertiseService("nullify_demand", &SrRobotLib::nullify_demand_callback, this);
116@@ -57,7 +61,7 @@
117 // using FORCE control if no parameters are set
118 control_type_.control_type = sr_robot_msgs::ControlType::FORCE;
119 std::string default_control_mode;
120- nh_tilde.param<std::string>("default_control_mode", default_control_mode, "force");
121+ nh_tilde.param<std::string>("default_control_mode", default_control_mode, "FORCE");
122 if( default_control_mode.compare("PWM") == 0 )
123 {
124 ROS_INFO("Using PWM control.");
125@@ -192,6 +196,20 @@
126
127 void SrRobotLib::build_motor_command(ETHERCAT_DATA_STRUCTURE_0200_PALM_EDC_COMMAND* command)
128 {
129+ //Mutual exclusion with the change_control_type service. We have to wait until the control_type_ variable has been set.
130+ boost::mutex::scoped_lock l(*lock_command_sending_);
131+
132+ if(control_type_changed_flag_)
133+ {
134+ if(!change_control_parameters(control_type_.control_type))
135+ {
136+ ROS_FATAL("Changing control parameters failed. Stopping real time loop to protect the robot.");
137+ exit(EXIT_FAILURE);
138+ }
139+
140+ control_type_changed_flag_ = false;
141+ }
142+
143 if (motor_current_state == operation_mode::device_update_state::INITIALIZATION)
144 {
145 motor_current_state = motor_updater_->build_init_command(command);
146@@ -1012,21 +1030,94 @@
147 if( (request.control_type.control_type != sr_robot_msgs::ControlType::PWM) &&
148 (request.control_type.control_type != sr_robot_msgs::ControlType::FORCE) )
149 {
150+ std::string ctrl_type_text = "";
151+ if(control_type_.control_type == sr_robot_msgs::ControlType::FORCE)
152+ ctrl_type_text = "FORCE";
153+ else
154+ ctrl_type_text = "PWM";
155+
156 ROS_ERROR_STREAM(" The value you specified for the control type (" << request.control_type
157- << ") is incorrect. Using FORCE control.");
158-
159- control_type_.control_type = sr_robot_msgs::ControlType::FORCE;
160+ << ") is incorrect. Using " << ctrl_type_text << " control.");
161
162 response.result = control_type_;
163 return false;
164 }
165
166- control_type_ = request.control_type;
167+ if(control_type_.control_type != request.control_type.control_type)
168+ {
169+ //Mutual exclusion with the build_motor_command() function. We have to wait until the current motor command has been built.
170+ boost::mutex::scoped_lock l(*lock_command_sending_);
171+
172+ ROS_WARN("Changing control type");
173+
174+ control_type_ = request.control_type;
175+ //Flag to signal that there has been a change in the value of control_type_ and certain actions are required.
176+ //The flag is set in the callback function of the change_control_type_ service.
177+ //The flag is checked in build_motor_command() and the necessary actions are taken there.
178+ //These actions involve calling services in the controller manager and all the active controllers. This is the
179+ //reason why we don't do it directly in the callback function. As we use a single thread to serve the callbacks,
180+ //doing so would cause a deadlock, thus we do it in the realtime loop thread instead.
181+ control_type_changed_flag_ = true;
182+ }
183
184 response.result = control_type_;
185 return true;
186 }
187
188+ bool SrRobotLib::change_control_parameters(int16_t control_type)
189+ {
190+ bool success = true;
191+ std::string env_variable;
192+ std::string param_value;
193+
194+ if( control_type == sr_robot_msgs::ControlType::PWM)
195+ {
196+ env_variable = "PWM_CONTROL=1";
197+ param_value = "PWM";
198+ }
199+ else
200+ {
201+ env_variable = "PWM_CONTROL=0";
202+ param_value = "FORCE";
203+ }
204+
205+ int result = system((env_variable + " roslaunch sr_ethercat_hand_config sr_edc_default_controllers.launch").c_str());
206+
207+ if(result == 0)
208+ {
209+ ROS_WARN("New parameters loaded successfully on Parameter Server");
210+
211+ nh_tilde.setParam("default_control_mode", param_value);
212+
213+ ros::ServiceClient list_ctrl_client = nh_tilde.serviceClient<pr2_mechanism_msgs::ListControllers>("/pr2_controller_manager/list_controllers");
214+ pr2_mechanism_msgs::ListControllers controllers_list;
215+
216+ if (list_ctrl_client.call(controllers_list))
217+ {
218+ for(unsigned int i=0; i < controllers_list.response.controllers.size(); ++i)
219+ {
220+ ros::ServiceClient reset_gains_client = nh_tilde.serviceClient<std_srvs::Empty>("/" + controllers_list.response.controllers.at(i) + "/reset_gains");
221+ std_srvs::Empty empty_message;
222+ if (!reset_gains_client.call(empty_message))
223+ {
224+ ROS_ERROR_STREAM("Failed to reset gains for controller: " << controllers_list.response.controllers.at(i));
225+ return false;
226+ }
227+ }
228+ }
229+ else
230+ {
231+ return false;
232+ }
233+ }
234+ else
235+ {
236+ return false;
237+ }
238+
239+ return success;
240+ }
241+
242 bool SrRobotLib::motor_system_controls_callback_( sr_robot_msgs::ChangeMotorSystemControls::Request& request,
243 sr_robot_msgs::ChangeMotorSystemControls::Response& response )
244 {

Subscribers

People subscribed via source and target branches

to all changes: