diff --git a/darwin_apps/launch/action_client_sim.launch b/darwin_apps/launch/action_client_sim.launch
index 79a75f965410a1c9f6efd1a7b7dae3b80ecf7bf3..3c42daf4734b4f8f30b3f87d1ede152e5f192de0 100644
--- a/darwin_apps/launch/action_client_sim.launch
+++ b/darwin_apps/launch/action_client_sim.launch
@@ -12,9 +12,9 @@
type="action_client"
output="screen"
ns="/darwin">
- <remap from="/darwin/motion"
+ <remap from="/darwin/action_client/action_client/motion_action"
to="/darwin/robot/motion_action"/>
- <remap from="/darwin/action_client/set_servo_modules"
+ <remap from="/darwin/action_client/action_client/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
</node>
diff --git a/darwin_apps/launch/all_clients_sim.launch b/darwin_apps/launch/all_clients_sim.launch
index 76d06249bbc90c065cbbb3d9a64e462564b1af93..f888808ef302b795ed4d3b04bd9ff09bdec97329 100644
--- a/darwin_apps/launch/all_clients_sim.launch
+++ b/darwin_apps/launch/all_clients_sim.launch
@@ -12,10 +12,10 @@
type="action_client"
output="screen"
ns="/darwin">
- <remap from="/darwin/motion"
- to="/darwin/robot/motion_action"/>
- <remap from="/darwin/action_client/set_servo_modules"
- to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/motion"
+ to="/darwin/robot/motion_action"/> <!--Action client: action_client_alg_node.cpp --><!--Action server darwin_driver -->
+ <remap from="/darwin/action_client/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/> <!--Client --><!--Service: darwin_driver -->
</node>
<!-- launch the action client node -->
@@ -24,10 +24,10 @@
type="joints_client"
output="screen"
ns="/darwin">
- <remap from="/darwin/joint_trajectory"
- to="/darwin/robot/joint_trajectory"/>
+ <remap from="/darwin/joint_trajectory"
+ to="/darwin/robot/joint_trajectory"/> <!--Action client --><!--Action server: darwin_driver -->
<remap from="/darwin/joints_client/set_servo_modules"
- to="/darwin/robot/set_servo_modules"/>
+ to="/darwin/robot/set_servo_modules"/> <!--Server: darwin_driver -->
</node>
<!-- launch the walk client node -->
@@ -53,12 +53,12 @@
<param name="FOOT_HEIGHT" value="0.04"/>
<param name="MAX_VEL" value="0.01"/>
<param name="MAX_ROT_VEL" value="0.01"/>
- <remap from="/darwin/walk_client/cmd_vel"
- to="/darwin/robot/cmd_vel"/>
+ <remap from="/darwin/walk_client/cmd_vel"
+ to="/darwin/robot/cmd_vel"/> <!--Publisher --><!--Subscriber: darwin_driver -->
<remap from="/darwin/walk_client/set_walk_params"
- to="/darwin/robot/set_walk_params"/>
+ to="/darwin/robot/set_walk_params"/> <!--Service: darwin_driver -->
<remap from="/darwin/walk_client/set_servo_modules"
- to="/darwin/robot/set_servo_modules"/>
+ to="/darwin/robot/set_servo_modules"/> <!--Service: darwin_driver -->
</node>
<!-- launch the action client node -->
@@ -68,15 +68,15 @@
output="screen"
ns="/darwin">
<remap from="/darwin/head_follow_target"
- to="/darwin/robot/head_follow_target"/>
- <remap from="/darwin/head_tracking_client/head_target"
- to="/darwin/robot/head_target"/>
- <remap from="/darwin/head_tracking_client/set_servo_modules"
- to="/darwin/robot/set_servo_modules"/>
+ to="/darwin/robot/head_follow_target"/> <!--Action server: darwin_driver -->
+ <remap from="/darwin/head_tracking_client/head_target"
+ to="/darwin/robot/head_target"/> <!--Publisher --><!--Subscriber: darwin_driver -->
+ <remap from="/darwin/head_tracking_client/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/> <!--Client --><!--Service: darwin_driver -->
<remap from="/darwin/head_tracking_client/set_pan_pid"
to="/darwin/robot/set_pan_pid"/>
<remap from="/darwin/head_tracking_client/set_tilt_pid"
- to="/darwin/robot/set_tilt_pid"/>
+ to="/darwin/robot/set_tilt_pid"/> <!--Service: darwin_driver -->
</node>
<!-- launch dynamic reconfigure -->
diff --git a/darwin_apps/launch/head_tracking_client_sim.launch b/darwin_apps/launch/head_tracking_client_sim.launch
index 60a76c278af7830ff542b2762cdede2902e5b173..f1bb5f02915c751a64716a15d998c50531262fc7 100644
--- a/darwin_apps/launch/head_tracking_client_sim.launch
+++ b/darwin_apps/launch/head_tracking_client_sim.launch
@@ -12,16 +12,20 @@
type="head_tracking_client"
output="screen"
ns="/darwin">
- <remap from="/darwin/head_follow_target"
+ <remap from="/darwin/head_tracking_client/head_tracking_client/head_tracking_action"
to="/darwin/robot/head_follow_target"/>
- <remap from="/darwin/head_tracking_client/head_target"
+ <remap from="/darwin/head_tracking_client/head_tracking_client/head_target"
to="/darwin/robot/head_target"/>
- <remap from="/darwin/head_tracking_client/set_servo_modules"
+ <remap from="/darwin/head_tracking_client/head_tracking_client/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
- <remap from="/darwin/head_tracking_client/set_pan_pid"
+ <remap from="/darwin/head_tracking_client/head_tracking_client/set_pan_pid"
to="/darwin/robot/set_pan_pid"/>
- <remap from="/darwin/head_tracking_client/set_tilt_pid"
+ <remap from="/darwin/head_tracking_client/head_tracking_client/get_pan_pid"
+ to="/darwin/robot/get_pan_pid"/>
+ <remap from="/darwin/head_tracking_client/head_tracking_client/set_tilt_pid"
to="/darwin/robot/set_tilt_pid"/>
+ <remap from="/darwin/head_tracking_client/head_tracking_client/get_tilt_pid"
+ to="/darwin/robot/get_tilt_pid"/>
</node>
<!-- launch dynamic reconfigure -->
diff --git a/darwin_apps/launch/joint_trajectory_client_sim.launch b/darwin_apps/launch/joint_trajectory_client_sim.launch
index fa12b925f46e930397d30ed9649ef1f5a8aa5da5..6c8ce1512abf3e85e4a3f0c01c2b1597499647a4 100644
--- a/darwin_apps/launch/joint_trajectory_client_sim.launch
+++ b/darwin_apps/launch/joint_trajectory_client_sim.launch
@@ -12,10 +12,10 @@
type="joints_client"
output="screen"
ns="/darwin">
- <remap from="/darwin/joint_trajectory"
- to="/darwin/robot/joint_trajectory"/>
- <remap from="/darwin/joints_client/set_servo_modules"
- to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/joint_trajectory" <!-- -->
+ to="/darwin/robot/joint_trajectory"/> <!--Action server: darwin_driver_node.cpp -->
+ <remap from="/darwin/joints_client/set_servo_modules" <!--Client: joints_client_alg_node.cpp -->
+ to="/darwin/robot/set_servo_modules"/> <!-- Service: darwin_driver_node.cpp -->
</node>
<!-- launch dynamic reconfigure -->
diff --git a/darwin_apps/launch/joints_cartesian_client_sim.launch b/darwin_apps/launch/joints_cartesian_client_sim.launch
new file mode 100644
index 0000000000000000000000000000000000000000..e4925c826be1ff010f467f76a5f986df4f4f5915
--- /dev/null
+++ b/darwin_apps/launch/joints_cartesian_client_sim.launch
@@ -0,0 +1,44 @@
+<launch>
+
+ <arg name="robot" default="darwin" />
+
+ <include file="$(find darwin_description)/launch/darwin_sim.launch">
+ <arg name="robot" value="$(arg robot)" />
+ </include>
+
+ <!-- launch the action client node -->
+ <node name="joints_cart_client"
+ pkg="joints_cart_client"
+ type="joints_cart_client"
+ output="screen"
+ ns="/darwin">
+ <remap from="/darwin/joints_cart_client/joints_cart_client/joints_action"
+ to="/darwin/robot/joint_trajectory"/>
+ <remap from="/darwin/joints_cart_client/joints_cart_client/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/joints_cart_client/joints_cart_client/get_arm_ik"
+ to="/darwin/left_arm_kinematics/get_ik"/>
+ <remap from="/darwin/joints_cart_client/joints_cart_client/arm_ik_set_params"
+ to="/darwin/left_arm_kinematics/set_parameters"/>
+ <remap from="/darwin/joints_cart_client/joints_cart_client/joint_states"
+ to="/darwin/joint_states"/>
+ </node>
+
+ <!-- load the controllers -->
+ <node name="left_arm_kinematics"
+ pkg="darwin_arm_kinematics"
+ type="darwin_arm_kinematics"
+ output="screen"
+ ns="/darwin">
+ <param name="chain_file" value="$(find darwin_arm_kinematics)/config/left_arm.xml"/>
+ <param name="tf_prefix" value="/darwin"/>
+ <param name="urdf_param" value="robot_description"/>
+ <param name="arm" value="0"/>
+ </node>
+
+ <!-- launch dynamic reconfigure -->
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+ output="screen"/>
+
+</launch>
+
diff --git a/darwin_apps/launch/qr_head_tracking.launch b/darwin_apps/launch/qr_head_tracking.launch
index 835462aaff6c7a0b29975668f71bd85f82f07e24..46347c8f9c6a33cd1d4c14e0d047579d51bd563c 100644
--- a/darwin_apps/launch/qr_head_tracking.launch
+++ b/darwin_apps/launch/qr_head_tracking.launch
@@ -29,10 +29,13 @@
<remap from="/darwin/qr_head_tracking/joint_states"
to="/darwin/joint_states"/>
<remap from="/darwin/qr_head_tracking/qr_pose"
- to="/darwin/qr_detector/qr_pose"/>
+ to="/qr_detector/qr_pose"/>
</node>
- <include file="$(find darwin_apps)/launch/xs_camera_qr.launch"/>
+<!-- <include file="$(find darwin_apps)/launch/xs_camera_qr.launch"/>-->
+ <include file="$(find qr_detector)/launch/tracker_darwin.launch"/>
</launch>
+<!-- <remap from="/darwin/qr_head_tracking/qr_pose"
+ to="/darwin/qr_detector/qr_pose"/> -->
\ No newline at end of file
diff --git a/darwin_apps/launch/walk_client_sim.launch b/darwin_apps/launch/walk_client_sim.launch
index 3b0b10b9e8837c4f10277771e76eac76e01d6e88..800df6d1b0c4de168b7df202e3fc52e0c1d4b9dd 100644
--- a/darwin_apps/launch/walk_client_sim.launch
+++ b/darwin_apps/launch/walk_client_sim.launch
@@ -29,12 +29,16 @@
<param name="FOOT_HEIGHT" value="0.04"/>
<param name="MAX_VEL" value="0.01"/>
<param name="MAX_ROT_VEL" value="0.01"/>
- <remap from="/darwin/walk_client/cmd_vel"
+ <remap from="/darwin/walk_client/walk_client/cmd_vel"
to="/darwin/robot/cmd_vel"/>
- <remap from="/darwin/walk_client/set_walk_params"
+ <remap from="/darwin/walk_client/walk_client/set_walk_params"
to="/darwin/robot/set_walk_params"/>
- <remap from="/darwin/walk_client/set_servo_modules"
+ <remap from="/darwin/walk_client/walk_client/get_walk_params"
+ to="/darwin/robot/get_walk_params"/>
+ <remap from="/darwin/walk_client/walk_client/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/walk_client/walk_client/joint_states"
+ to="/darwin/joint_states"/>
</node>
<!-- launch dynamic reconfigure -->
diff --git a/darwin_arm_kinematics/CMakeLists.txt b/darwin_arm_kinematics/CMakeLists.txt
index d62d94701fcfa0d8ad9fb9ce01ddf5d08bba938b..c9b25f85ea74d60b50de69cb24fec4ecddfd3171 100644
--- a/darwin_arm_kinematics/CMakeLists.txt
+++ b/darwin_arm_kinematics/CMakeLists.txt
@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm moveit_msgs tf cmake_modules)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm moveit_msgs tf cmake_modules urdf)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -63,7 +63,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
- CATKIN_DEPENDS iri_base_algorithm moveit_msgs tf
+ CATKIN_DEPENDS iri_base_algorithm moveit_msgs tf urdf
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
diff --git a/darwin_arm_kinematics/cfg/DarwinArmKinematics.cfg b/darwin_arm_kinematics/cfg/DarwinArmKinematics.cfg
index 2e61038656c8ba6cadeef18f53968e4b316380b5..ef41117c364d93c0200de0ec4f1baa2e9d195a61 100755
--- a/darwin_arm_kinematics/cfg/DarwinArmKinematics.cfg
+++ b/darwin_arm_kinematics/cfg/DarwinArmKinematics.cfg
@@ -46,6 +46,11 @@ gen.const("Right", int_t, 1, "Right arm"),
#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
gen.add("chain_file", str_t, 0, "Chain definition file", "")
gen.add("tf_prefix", str_t, 0, "Prefix for the TF", "")
+gen.add("urdf_param", str_t, 0, "Parameter name of the URDF file","")
+gen.add("max_ik_iter", int_t, 0, "Maximum number of iterations to find a solution of the inverse kinematics",10, 1, 50)
+gen.add("max_ik_tol", double_t, 0, "Maximum tolerance allowed to find a solution of the inverse kinematics",0.000001, 0.001, 0.1)
+gen.add("max_fk_iter", int_t, 0, "Maximum number of iterations to find a solution of the forward kinematics",10, 1, 50)
+gen.add("max_fk_tol", double_t, 0, "Maximum tolerance allowed to find a solution of the forward kinematics",0.000001, 0.001, 0.1)
gen.add("arm", int_t, 0, "Arm identifier", 0, 0, 1, edit_method=enum_arm)
exit(gen.generate(PACKAGE, "DarwinArmKinematicsAlgorithm", "DarwinArmKinematics"))
diff --git a/darwin_arm_kinematics/config/left_arm.xml b/darwin_arm_kinematics/config/left_arm.xml
index f4e2cd1d671bf29f4a5464636acce0c28296e182..ed8c57ee695daf4c3e96c4c632c9f84e6ad281db 100644
--- a/darwin_arm_kinematics/config/left_arm.xml
+++ b/darwin_arm_kinematics/config/left_arm.xml
@@ -3,6 +3,7 @@
<darwin_kin_chain xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:noNamespaceSchemaLocation="darwin_kin_chain.xsd">
<joint>
+ <name>j_shoulder_pitch_l</name>
<params>
<a>-0.016</a>
<alpha>1.5707</alpha>
@@ -13,6 +14,7 @@
<min_angle>-1.5707</min_angle>
</joint>
<joint>
+ <name>j_shoulder_roll_l</name>
<params>
<a>-0.06</a>
<alpha>1.5707</alpha>
@@ -23,21 +25,12 @@
<min_angle>-1.5707</min_angle>
</joint>
<joint>
+ <name>j_elbow_l</name>
<params>
- <a>-0.02165</a>
+ <a>-0.08165</a>
<alpha>1.5707</alpha>
<d>0</d>
- <theta>0</theta>
- </params>
- <max_angle>1.5707</max_angle>
- <min_angle>-1.5707</min_angle>
- </joint>
- <joint>
- <params>
- <a>0</a>
- <alpha>0</alpha>
- <d>0.056</d>
- <theta>0</theta>
+ <theta>1.5707</theta>
</params>
<max_angle>1.5707</max_angle>
<min_angle>-1.5707</min_angle>
diff --git a/darwin_arm_kinematics/config/right_arm.xml b/darwin_arm_kinematics/config/right_arm.xml
index 1b4e6a1411d42668d7a858f1b9670853fb92bc4e..92336f118de340b35a0e68e5d67cabb6a930324b 100644
--- a/darwin_arm_kinematics/config/right_arm.xml
+++ b/darwin_arm_kinematics/config/right_arm.xml
@@ -3,6 +3,7 @@
<darwin_kin_chain xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:noNamespaceSchemaLocation="darwin_kin_chain.xsd">
<joint>
+ <name>j_shoulder_pitch_l</name>
<params>
<a>-0.016</a>
<alpha>-1.5707</alpha>
@@ -13,6 +14,7 @@
<min_angle>-1.5707</min_angle>
</joint>
<joint>
+ <name>j_shoulder_roll_l</name>
<params>
<a>-0.06</a>
<alpha>1.5707</alpha>
@@ -23,21 +25,12 @@
<min_angle>-1.5707</min_angle>
</joint>
<joint>
+ <name>j_elbow_l</name>
<params>
- <a>-0.02165</a>
+ <a>-0.08165</a>
<alpha>1.5707</alpha>
<d>0</d>
- <theta>0</theta>
- </params>
- <max_angle>1.5707</max_angle>
- <min_angle>-1.5707</min_angle>
- </joint>
- <joint>
- <params>
- <a>0</a>
- <alpha>0</alpha>
- <d>0.056</d>
- <theta>0</theta>
+ <theta>1.5707</theta>
</params>
<max_angle>1.5707</max_angle>
<min_angle>-1.5707</min_angle>
diff --git a/darwin_arm_kinematics/include/darwin_arm_kinematics_alg_node.h b/darwin_arm_kinematics/include/darwin_arm_kinematics_alg_node.h
index 7c944383b25e701c3e7b2608f2a1d52f6195280e..148bd07f6e34dc3721aa9c5f0a03c28cfd6f1572 100644
--- a/darwin_arm_kinematics/include/darwin_arm_kinematics_alg_node.h
+++ b/darwin_arm_kinematics/include/darwin_arm_kinematics_alg_node.h
@@ -71,6 +71,7 @@ class DarwinArmKinematicsAlgNode : public algorithm_base::IriBaseAlgorithm<Darwi
Config config;
CDarwinArmKinematics chain;
+ bool initialized;
tf::TransformListener tf_listener_;
tf::TransformBroadcaster tf_broadcaster_;
public:
@@ -127,6 +128,7 @@ class DarwinArmKinematicsAlgNode : public algorithm_base::IriBaseAlgorithm<Darwi
*/
void addNodeDiagnostics(void);
+ void update_chain(void);
// [diagnostic functions]
// [test functions]
diff --git a/darwin_arm_kinematics/launch/left_arm_kin.launch b/darwin_arm_kinematics/launch/left_arm_kin.launch
index 0a075e12b5cb3a6382b891d9fc480b716685b90d..4d2beefa14611eee3afee7dee49dc87ee5559372 100644
--- a/darwin_arm_kinematics/launch/left_arm_kin.launch
+++ b/darwin_arm_kinematics/launch/left_arm_kin.launch
@@ -8,6 +8,7 @@
ns="/darwin">
<param name="chain_file" value="$(find darwin_arm_kinematics)/config/left_arm.xml"/>
<param name="tf_prefix" value="/darwin"/>
+ <param name="urdf_param" value="robot_description"/>
<param name="arm" value="0"/>
</node>
</launch>
diff --git a/darwin_arm_kinematics/launch/left_arm_kin_test.launch b/darwin_arm_kinematics/launch/left_arm_kin_test.launch
new file mode 100644
index 0000000000000000000000000000000000000000..f399a016e0d2eca6f4d198b8fef8351745d2e91b
--- /dev/null
+++ b/darwin_arm_kinematics/launch/left_arm_kin_test.launch
@@ -0,0 +1,21 @@
+<launch>
+
+ <arg name="robot" default="darwin" />
+
+ <!-- Convert an xacro and put on parameter server -->
+ <param name="robot_description"
+ command="$(find xacro)/xacro.py '$(find darwin_description)/urdf/$(arg robot).xacro'" />
+
+ <!-- load the controllers -->
+ <node name="left_arm_kinematics"
+ pkg="darwin_arm_kinematics"
+ type="darwin_arm_kinematics"
+ output="screen"
+ ns="/darwin">
+ <param name="chain_file" value="$(find darwin_arm_kinematics)/config/left_arm.xml"/>
+ <param name="tf_prefix" value="/darwin"/>
+ <param name="urdf_param" value="robot_description"/>
+ <param name="arm" value="0"/>
+ </node>
+</launch>
+
diff --git a/darwin_arm_kinematics/launch/right_arm_kin.launch b/darwin_arm_kinematics/launch/right_arm_kin.launch
index 18a643ba813e04de62cfdd86adf74fdfcab8f20c..4dc30670be5c925446653c610706334c6dca3519 100644
--- a/darwin_arm_kinematics/launch/right_arm_kin.launch
+++ b/darwin_arm_kinematics/launch/right_arm_kin.launch
@@ -8,6 +8,7 @@
ns="/darwin">
<param name="chain_file" value="$(find darwin_arm_kinematics)/config/right_arm.xml"/>
<param name="tf_prefix" value="/darwin"/>
+ <param name="urdf_param" value="robot_description"/>
<param name="arm" value="1"/>
</node>
</launch>
diff --git a/darwin_arm_kinematics/package.xml b/darwin_arm_kinematics/package.xml
index 1a57ab20e012d2b3652632643005684fc8da88cf..51273a8b12a1f0175a9c880dc4ce701ef764a64b 100644
--- a/darwin_arm_kinematics/package.xml
+++ b/darwin_arm_kinematics/package.xml
@@ -43,11 +43,12 @@
<build_depend>iri_base_algorithm</build_depend>
<build_depend>moveit_msgs</build_depend>
<build_depend>tf</build_depend>
+ <build_depend>urdf</build_depend>
<build_depend>cmake_modules</build_depend>
<run_depend>iri_base_algorithm</run_depend>
<run_depend>moveit_msgs</run_depend>
<run_depend>tf</run_depend>
-
+ <run_depend>urdf</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
diff --git a/darwin_arm_kinematics/src/darwin_arm_kinematics_alg_node.cpp b/darwin_arm_kinematics/src/darwin_arm_kinematics_alg_node.cpp
index bd41f3acae3e73f32ddb30c11115ef7a7861a8e6..f54adc0331f987060b48db3ea9bf8b147c3ca64e 100644
--- a/darwin_arm_kinematics/src/darwin_arm_kinematics_alg_node.cpp
+++ b/darwin_arm_kinematics/src/darwin_arm_kinematics_alg_node.cpp
@@ -1,6 +1,8 @@
#include "darwin_arm_kinematics_alg_node.h"
#include "darwin_robot_exceptions.h"
#include <tf/tf.h>
+#include <urdf/model.h>
+#include <moveit_msgs/MoveItErrorCodes.h>
DarwinArmKinematicsAlgNode::DarwinArmKinematicsAlgNode(void) :
algorithm_base::IriBaseAlgorithm<DarwinArmKinematicsAlgorithm>(),
@@ -29,13 +31,14 @@ DarwinArmKinematicsAlgNode::DarwinArmKinematicsAlgNode(void) :
this->private_node_handle_.getParam("chain_file",this->config.chain_file);
this->private_node_handle_.getParam("tf_prefix",this->config.tf_prefix);
+ this->private_node_handle_.getParam("urdf_param",this->config.urdf_param);
this->private_node_handle_.getParam("arm",this->config.arm);
- //load the chain file
- try{
- this->chain.load_chain(this->config.chain_file);
- }catch(CException &e){
- ROS_INFO("%s",e.what().c_str());
- }
+ this->private_node_handle_.getParam("max_ik_iter",this->config.max_ik_iter);
+ this->private_node_handle_.getParam("max_ik_tol",this->config.max_ik_tol);
+ this->private_node_handle_.getParam("max_fk_iter",this->config.max_fk_iter);
+ this->private_node_handle_.getParam("max_fk_tol",this->config.max_fk_tol);
+
+ this->initialized=false;
}
DarwinArmKinematicsAlgNode::~DarwinArmKinematicsAlgNode(void)
@@ -49,25 +52,29 @@ void DarwinArmKinematicsAlgNode::mainNodeThread(void)
{
geometry_msgs::TransformStamped kin_trans_msg;
- kin_trans_msg.header.stamp = ros::Time::now();
- if(this->config.arm==0)//left arm
+ if(this->initialized)
{
- kin_trans_msg.header.frame_id = this->config.tf_prefix + "/base_link";
- kin_trans_msg.child_frame_id = this->config.tf_prefix + "/base_arm_l";
- kin_trans_msg.transform.rotation=tf::createQuaternionMsgFromRollPitchYaw(-1.5707,0,-1.5707);
- kin_trans_msg.transform.translation.x=0.06;
+ kin_trans_msg.header.stamp = ros::Time::now();
+ if(this->config.arm==0)//left arm
+ {
+ kin_trans_msg.header.frame_id = this->config.tf_prefix + "/base_link";
+ kin_trans_msg.child_frame_id = this->config.tf_prefix + "/base_arm_l";
+ kin_trans_msg.transform.rotation=tf::createQuaternionMsgFromRollPitchYaw(-1.5707,0,-1.5707);
+ kin_trans_msg.transform.translation.x=0.06;
+ }
+ else
+ {
+ kin_trans_msg.header.frame_id = this->config.tf_prefix + "/base_link";
+ kin_trans_msg.child_frame_id = this->config.tf_prefix + "/base_arm_r";
+ kin_trans_msg.transform.rotation=tf::createQuaternionMsgFromRollPitchYaw(+1.5707,0,-1.5707);
+ kin_trans_msg.transform.translation.x=-0.06;
+ }
+ kin_trans_msg.transform.translation.y=0;
+ kin_trans_msg.transform.translation.z=0;
+ this->tf_broadcaster_.sendTransform(kin_trans_msg);
}
else
- {
- kin_trans_msg.header.frame_id = this->config.tf_prefix + "/base_link";
- kin_trans_msg.child_frame_id = this->config.tf_prefix + "/base_arm_r";
- kin_trans_msg.transform.rotation=tf::createQuaternionMsgFromRollPitchYaw(+1.5707,0,-1.5707);
- kin_trans_msg.transform.translation.x=-0.06;
- }
- kin_trans_msg.transform.translation.y=0;
- kin_trans_msg.transform.translation.z=0;
- this->tf_broadcaster_.sendTransform(kin_trans_msg);
-
+ this->update_chain();
// [fill msg structures]
// [fill srv structure and make request to the server]
@@ -146,6 +153,7 @@ bool DarwinArmKinematicsAlgNode::get_ikCallback(moveit_msgs::GetPositionIK::Requ
geometry_msgs::PointStamped point_in,point_out;
ros::Time transform_time=ros::Time::now();
std::string origin_frame,target_frame;
+ std::vector<std::string> joint_names;
std::vector<double> joints,pos,rot;
bool tf_exists;
@@ -179,17 +187,26 @@ bool DarwinArmKinematicsAlgNode::get_ikCallback(moveit_msgs::GetPositionIK::Requ
rot[2]=0;
this->chain.get_inverse_kinematics(pos,rot,joints);
res.solution.joint_state.position.resize(joints.size());
+ res.solution.joint_state.name.resize(joints.size());
+ joint_names=this->chain.get_joint_names();
for(i=0;i<joints.size();i++)
+ {
res.solution.joint_state.position[i]=joints[i];
+ res.solution.joint_state.name[i]=joint_names[i];
+ }
+ res.error_code.val=moveit_msgs::MoveItErrorCodes::SUCCESS;
this->alg_.unlock();
return true;
}
else
{
+ res.error_code.val=moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
this->alg_.unlock();
return false;
}
}catch(CException &e){
+ res.error_code.val=moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
+ this->alg_.unlock();
ROS_INFO("%s",e.what().c_str());
return false;
}
@@ -212,11 +229,58 @@ void DarwinArmKinematicsAlgNode::get_ik_mutex_exit(void)
void DarwinArmKinematicsAlgNode::node_config_update(Config &config, uint32_t level)
{
this->alg_.lock();
+ if(this->config.chain_file!=config.chain_file)
+ this->initialized=false;
this->config=config;
+ /* update parameters */
+ this->chain.set_ik_max_attempts(config.max_ik_iter);
+ this->chain.set_ik_max_tolerance(config.max_ik_tol);
+ this->chain.set_fk_max_iterations(config.max_fk_iter);
+ this->chain.set_fk_max_tolerance(config.max_fk_tol);
+ this->alg_.unlock();
+}
+
+void DarwinArmKinematicsAlgNode::update_chain(void)
+{
+ urdf::Model model;
+ std::vector< boost::shared_ptr<urdf::Link> > links;
+ std::vector<std::string> chain_joints;
+ unsigned int i=0,j=0;
+
+ this->alg_.lock();
try{
this->chain.load_chain(this->config.chain_file);
+ if(!model.initParam(this->config.urdf_param))
+ {
+ this->initialized=false;
+ ROS_ERROR("Impossible to load the URDF description");
+ }
+ else
+ {
+ chain_joints=this->chain.get_joint_names();
+ model.getLinks(links);
+ /* check the joints in the chain file */
+ for(i=0;i<chain_joints.size();i++)
+ {
+ for(j=0;j<links.size();j++)
+ {
+ if(links[j]->parent_joint!=NULL)
+ if(chain_joints[i]==links[j]->parent_joint->name)
+ break;
+ }
+ if(j==links.size())
+ {
+ ROS_ERROR_STREAM("Joint " << chain_joints[i] << " not present in URDF file");
+ this->initialized=false;
+ this->alg_.unlock();
+ return;
+ }
+ }
+ this->initialized=true;
+ }
}catch(CException &e){
- ROS_INFO("%s",e.what().c_str());
+ this->initialized=false;
+ ROS_ERROR("%s",e.what().c_str());
}
this->alg_.unlock();
}
diff --git a/darwin_controller/include/darwin_controller_impl.h b/darwin_controller/include/darwin_controller_impl.h
index 43d2338f9e058d464e044351067d46f3689efb48..f3c9975d4356abd0a6228badaaf366be57c74ad8 100644
--- a/darwin_controller/include/darwin_controller_impl.h
+++ b/darwin_controller/include/darwin_controller_impl.h
@@ -72,12 +72,12 @@ namespace darwin_controller
XmlRpcValue xml_array;
if (!nh.getParam(param_name, xml_array))
{
- ROS_ERROR_STREAM("Could not find '" << param_name << "' parameter (namespace: " << nh.getNamespace() << ").");
+ ROS_ERROR_STREAM("CDarwinSim : Could not find '" << param_name << "' parameter (namespace: " << nh.getNamespace() << ").");
return std::vector<std::string>();
}
if (xml_array.getType() != XmlRpcValue::TypeArray)
{
- ROS_ERROR_STREAM("The '" << param_name << "' parameter is not an array (namespace: " <<
+ ROS_ERROR_STREAM("CDarwinSim : The '" << param_name << "' parameter is not an array (namespace: " <<
nh.getNamespace() << ").");
return std::vector<std::string>();
}
@@ -87,7 +87,7 @@ namespace darwin_controller
{
if (xml_array[i].getType() != XmlRpcValue::TypeString)
{
- ROS_ERROR_STREAM("The '" << param_name << "' parameter contains a non-string element (namespace: " <<
+ ROS_ERROR_STREAM("CDarwinSim : The '" << param_name << "' parameter contains a non-string element (namespace: " <<
nh.getNamespace() << ").");
return std::vector<std::string>();
}
@@ -106,7 +106,7 @@ namespace darwin_controller
{
if (!urdf->initString(urdf_str))
{
- ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter (namespace: " <<
+ ROS_ERROR_STREAM("CDarwinSim : Failed to parse URDF contained in '" << param_name << "' parameter (namespace: " <<
nh.getNamespace() << ").");
return boost::shared_ptr<urdf::Model>();
}
@@ -114,7 +114,7 @@ namespace darwin_controller
// Check for robot_description in root
else if (!urdf->initParam("robot_description"))
{
- ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter");
+ ROS_ERROR_STREAM("CDarwinSim : Failed to parse URDF contained in '" << param_name << "' parameter");
return boost::shared_ptr<urdf::Model>();
}
return urdf;
@@ -133,7 +133,7 @@ namespace darwin_controller
}
else
{
- ROS_ERROR_STREAM("Could not find joint '" << joint_names[i] << "' in URDF model.");
+ ROS_ERROR_STREAM("CDarwinSim : Could not find joint '" << joint_names[i] << "' in URDF model.");
return std::vector<UrdfJointConstPtr>();
}
}
@@ -249,7 +249,7 @@ namespace darwin_controller
try {
joints_[i]=hw->getHandle(joint_names_[i]);
}catch(...){
- ROS_ERROR_STREAM_NAMED(name_,"Could not find joint '" << joint_names_[i] << "' in '" <<
+ ROS_ERROR_STREAM("CDarwinSim : Could not find joint '" << joint_names_[i] << "' in '" <<
this->getHardwareInterfaceType() << "'.");
return false;
}
@@ -260,17 +260,17 @@ namespace darwin_controller
this->pids_[i].reset(new control_toolbox::Pid());
if(!this->pids_[i]->init(joint_nh))
{
- ROS_WARN_STREAM("Failed to initialize PID gains from ROS parameter server.");
+ ROS_WARN_STREAM("CDarwinSim : Failed to initialize PID gains from ROS parameter server.");
return false;
}
// Whether a joint is continuous (ie. has angle wraparound)
const std::string not_if = urdf_joints[i]->type == urdf::Joint::CONTINUOUS ? "" : "non-";
- ROS_DEBUG_STREAM_NAMED(name_, "Found " << not_if << "continuous joint '" << joint_names_[i] << "' in '" << this->getHardwareInterfaceType() << "'.");
+ ROS_DEBUG_STREAM("CDarwinSim : Found " << not_if << "continuous joint '" << joint_names_[i] << "' in '" << this->getHardwareInterfaceType() << "'.");
}
- ROS_DEBUG_STREAM_NAMED(name_, "Initialized controller '" << name_ << "' with:" <<
+ ROS_DEBUG_STREAM("CDarwinSim : Initialized controller '" << name_ << "' with:" <<
"\n- Number of joints: " << joints_.size() <<
"\n- Hardware interface type: '" << this->getHardwareInterfaceType() << "'");
@@ -280,7 +280,7 @@ namespace darwin_controller
imu_init();
/* initialize motion modules */
manager_init(7800);// motion manager period in us
- ROS_INFO("motion manager initialized");
+ ROS_INFO("CDarwinSim : motion manager initialized");
/* go to the walk ready position */
for(unsigned int i=0;i<n_joints;++i)
{
@@ -305,13 +305,9 @@ namespace darwin_controller
/* get the actual simulation angles */
for(unsigned int i=0;i<this->joints_.size();++i)
real_angles[i]=joints_[i].getPosition();
-// if((current_time-last_time).toSec()>0.0078)
-// {
__HAL_TIM_SET_FLAG(TIM5,TIM_FLAG_CC1);
__HAL_TIM_SET_IT_SOURCE(TIM5,TIM_IT_CC1);
TIM5_IRQHandler();
-// last_time=current_time;
-// }
for (unsigned int i = 0; i < this->joints_.size(); ++i)
{
target_angles[i] = ((manager_servos[i+1].current_angle*3.14159)/180.0)/128.0;
@@ -326,13 +322,13 @@ namespace darwin_controller
template <class HardwareInterface>
void DarwinController<HardwareInterface>::motion_action_startCallback(const humanoid_common_msgs::humanoid_motionGoalConstPtr& goal)
{
- ROS_INFO("Start action");
+ ROS_INFO_STREAM("CDarwinSim : Start motion action " << goal->motion_id);
/* load a page and start execution */
if(!action_load_page(goal->motion_id))
- ROS_WARN("Impossible to load the desired page");
+ ROS_WARN("CDarwinSim : Impossible to load the desired page");
else
{
- ROS_INFO("Page loaded successfully");
+ ROS_INFO("CDarwinSim : Page loaded successfully");
action_start_page();
}
}
@@ -340,7 +336,7 @@ namespace darwin_controller
template <class HardwareInterface>
void DarwinController<HardwareInterface>::motion_action_stopCallback(void)
{
- ROS_INFO("Stop action");
+ ROS_INFO("CDarwinSim : Stop motion action");
action_stop_page();
}
@@ -382,7 +378,7 @@ namespace darwin_controller
uint8_t speed,accel;
uint32_t servos=0;
- ROS_INFO("Start action");
+ ROS_INFO("CDarwinSim : Start joint trajectory action");
if(goal->trajectory.points.size()==1)
{
for(i=0;i<goal->trajectory.joint_names.size();i++)
@@ -409,7 +405,7 @@ namespace darwin_controller
template <class HardwareInterface>
void DarwinController<HardwareInterface>::joint_trajectory_stopCallback(void)
{
- ROS_INFO("Stop action");
+ ROS_INFO("CDarwinSim : Stop joint trajectory action");
joint_motion_stop(0);
}
@@ -444,7 +440,7 @@ namespace darwin_controller
template <class HardwareInterface>
void DarwinController<HardwareInterface>::follow_target_startCallback(const humanoid_common_msgs::humanoid_follow_targetGoalConstPtr& goal)
{
- ROS_INFO("Start action");
+ ROS_INFO("CDarwinSim : Start head tracking action");
if(goal->pan_range.size()==2)
head_tracking_set_pan_range(((goal->pan_range[0]*180.0)/3.14159)*128.0,((goal->pan_range[1]*180.0)/3.14159)*128.0);
if(goal->tilt_range.size()==2)
@@ -457,7 +453,7 @@ namespace darwin_controller
template <class HardwareInterface>
void DarwinController<HardwareInterface>::follow_target_stopCallback(void)
{
- ROS_INFO("Stop action");
+ ROS_INFO("CDarwinSim : Stop head_tracking action");
head_tracking_stop();
}
@@ -495,7 +491,7 @@ namespace darwin_controller
template <class HardwareInterface>
bool DarwinController<HardwareInterface>::set_walk_paramsCallback(humanoid_common_msgs::set_walk_params::Request &req, humanoid_common_msgs::set_walk_params::Response &res)
{
- ROS_INFO("Setting walk parameters ...");
+ ROS_INFO("CDarwinSim : Setting walk parameters ...");
this->walk_access.enter();
ram_write_byte(DARWIN_WALK_SWING_RIGHT_LEFT,(unsigned char)(req.params.Y_SWAP_AMPLITUDE*1000.0));
ram_write_byte(DARWIN_WALK_SWING_TOP_DOWN,(unsigned char)(req.params.Z_SWAP_AMPLITUDE*1000.0));
@@ -527,7 +523,7 @@ namespace darwin_controller
unsigned char byte_value;
unsigned short int word_value;
- ROS_INFO("Getting current walk parameters ...");
+ ROS_INFO("CDarwinSim : Getting current walk parameters ...");
this->walk_access.enter();
ram_read_byte(DARWIN_WALK_SWING_RIGHT_LEFT,&byte_value);
res.params.Y_SWAP_AMPLITUDE=((double)byte_value)/1000.0;
@@ -559,11 +555,15 @@ namespace darwin_controller
res.params.STEP_FB_RATIO=((double)byte_value)/256.0;
ram_read_byte(DARWIN_WALK_FOOT_HEIGHT,&byte_value);
res.params.STEP_FB_RATIO=((double)byte_value)/1000.0;
+ ram_read_byte(DARWIN_WALK_FOOT_HEIGHT,&byte_value);
+ res.params.FOOT_HEIGHT=((double)byte_value)/1000.0;
ram_read_byte(DARWIN_WALK_MAX_VEL,&byte_value);
res.params.MAX_VEL=((double)byte_value)/1000.0;
ram_read_byte(DARWIN_WALK_MAX_ROT_VEL,&byte_value);
res.params.MAX_ROT_VEL=(((double)byte_value)*PI)/1440.0;
this->walk_access.exit();
+
+ return true;
}
/************************************ get_walk_params *******************************************/
@@ -573,7 +573,7 @@ namespace darwin_controller
{
unsigned int i=0,j=0;
- ROS_INFO("Setting servo modules ...");
+ ROS_INFO("CDarwinSim : Setting servo modules ...");
for(i=0;i<req.names.size();i++)
{
for(j=0;j<MANAGER_MAX_NUM_SERVOS;j++)
@@ -606,7 +606,7 @@ namespace darwin_controller
{
uint16_t kp,ki,kd,i_clamp;
- ROS_INFO("Setting pan pid ...");
+ ROS_INFO("CDarwinSim : Setting pan pid ...");
kp=req.p*65536.0;
ki=req.i*65536.0;
kd=req.d*65536.0;
@@ -624,7 +624,7 @@ namespace darwin_controller
{
uint16_t kp,ki,kd,i_clamp;
- ROS_INFO("Getting pan pid ...");
+ ROS_INFO("CDarwinSim : Getting pan pid ...");
head_tracking_get_pan_pid(&kp,&ki,&kd,&i_clamp);
res.p=((float)kp)/65536.0;
res.i=((float)ki)/65536.0;
@@ -642,7 +642,7 @@ namespace darwin_controller
{
uint16_t kp,ki,kd,i_clamp;
- ROS_INFO("Setting tilt pid ...");
+ ROS_INFO("CDarwinSim : Setting tilt pid ...");
kp=req.p*65536.0;
ki=req.i*65536.0;
kd=req.d*65536.0;
@@ -660,7 +660,7 @@ namespace darwin_controller
{
uint16_t kp,ki,kd,i_clamp;
- ROS_INFO("Getting tilt pid ...");
+ ROS_INFO("CDarwinSim : Getting tilt pid ...");
head_tracking_get_tilt_pid(&kp,&ki,&kd,&i_clamp);
res.p=((float)kp)/65536.0;
res.i=((float)ki)/65536.0;
@@ -708,7 +708,7 @@ namespace darwin_controller
ram_write_byte(DARWIN_WALK_STEP_DIRECTION,(unsigned char)(((msg->angular.z*1.44)/PI)*period));
if(!is_walking())
{
- ROS_INFO("Start walking");
+ ROS_INFO("CDarwinSim : Start walking");
walking_start();
}
this->walk_timeout=ros::Time::now();
@@ -720,7 +720,7 @@ namespace darwin_controller
template <class HardwareInterface>
void DarwinController<HardwareInterface>::head_target_callback(const trajectory_msgs::JointTrajectoryPoint::ConstPtr& msg)
{
- ROS_INFO("New head target ...");
+ ROS_INFO("CDarwinSim : New head target ...");
if(msg->positions.size()==2)
{
head_tracking_set_target_pan(((msg->positions[0]*180.0)/3.14159)*128.0);
@@ -758,13 +758,13 @@ namespace darwin_controller
for(i=0;i<msg->name.size();i++)
{
if(msg->blink[i])
- std::cout << "blink led " << msg->name[i] << " with period " << msg->period_ms[i] << " ms" << std::endl;
+ ROS_INFO_STREAM("CDarwinSim : blink led " << msg->name[i] << " with period " << msg->period_ms[i] << " ms");
else if(msg->toggle[i])
- std::cout << "toggle led " << msg->name[i] << std::endl;
+ ROS_INFO_STREAM("CDarwinSim : toggle led " << msg->name[i]);
else if(msg->value[i])
- std::cout << "set led " << msg->name[i] << std::endl;
+ ROS_INFO_STREAM("CDarwinSim : set led " << msg->name[i]);
else
- std::cout << "clear led " << msg->name[i] << std::endl;
+ ROS_INFO_STREAM("CDarwinSim : clear led " << msg->name[i]);
}
}
/****************************************** LEDS *************************************************/
diff --git a/darwin_description/config/darwin_rviz.rviz b/darwin_description/config/darwin_rviz.rviz
index bb4fcf73d3f614dd455eea65b53d938b560bef71..3781f56c23b436652639f5f3510994755ddba1e5 100644
--- a/darwin_description/config/darwin_rviz.rviz
+++ b/darwin_description/config/darwin_rviz.rviz
@@ -9,9 +9,8 @@ Panels:
- /TF1/Frames1
- /TF1/Tree1
- /Environment1
- - /Environment1/Status1
Splitter Ratio: 0.744966
- Tree Height: 775
+ Tree Height: 655
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@@ -196,45 +195,7 @@ Visualization Manager:
Frame Timeout: 15
Frames:
All Enabled: true
- E125:
- Value: true
- E175:
- Value: true
- E225:
- Value: true
- E25:
- Value: true
- E75:
- Value: true
- N125:
- Value: true
- N125_cam:
- Value: true
- N175:
- Value: true
- N25:
- Value: true
- N75:
- Value: true
- N75_cam:
- Value: true
- S125:
- Value: true
- S175:
- Value: true
- S25:
- Value: true
- S75:
- Value: true
- W125:
- Value: true
- W175:
- Value: true
- W225:
- Value: true
- W25:
- Value: true
- W75:
+ darwin/base_footprint:
Value: true
darwin/base_link:
Value: true
@@ -268,8 +229,6 @@ Visualization Manager:
Value: true
darwin/neck:
Value: true
- darwin/odom:
- Value: true
darwin/right_arm_high:
Value: true
darwin/right_arm_low:
@@ -288,92 +247,49 @@ Visualization Manager:
Value: true
darwin/right_shoulder:
Value: true
- obstacle_base_fence_link:
- Value: true
- obstacle_base_link:
- Value: true
Marker Scale: 0.2
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
- obstacle_base_link:
- E125:
- {}
- E175:
- {}
- E225:
- {}
- E25:
- {}
- E75:
- {}
- N125:
- {}
- N175:
- {}
- N25:
- {}
- N75:
- {}
- S125:
- {}
- S175:
- {}
- S25:
- {}
- S75:
- {}
- W125:
- {}
- W175:
- {}
- W225:
+ darwin/base_link:
+ darwin/darwin_accel:
{}
- W25:
+ darwin/darwin_gyro:
{}
- W75:
+ darwin/darwin_imu_link:
{}
- darwin/odom:
+ darwin/left_leg_pelvis_yaw:
+ darwin/left_leg_pelvis_roll:
+ darwin/left_leg_pelvis_pitch:
+ darwin/left_leg_knee:
+ darwin/left_leg_ankle_pitch:
+ darwin/left_leg_ankle_roll:
+ {}
+ darwin/left_shoulder:
+ darwin/left_arm_high:
+ darwin/left_arm_low:
darwin/base_link:
darwin/darwin_accel:
{}
darwin/darwin_gyro:
{}
- darwin/darwin_imu_link:
+ darwin/neck:
+ darwin/head:
+ darwin/darwin_cam_link:
{}
- darwin/left_leg_pelvis_yaw:
- darwin/left_leg_pelvis_roll:
- darwin/left_leg_pelvis_pitch:
- darwin/left_leg_knee:
- darwin/left_leg_ankle_pitch:
- darwin/left_leg_ankle_roll:
- {}
- darwin/left_shoulder:
- darwin/left_arm_high:
- darwin/left_arm_low:
- {}
- darwin/neck:
- darwin/head:
- darwin/darwin_cam_link:
- N125_cam:
+ darwin/right_leg_pelvis_yaw:
+ darwin/right_leg_pelvis_roll:
+ darwin/right_leg_pelvis_pitch:
+ darwin/right_leg_knee:
+ darwin/right_leg_ankle_pitch:
+ darwin/right_leg_ankle_roll:
{}
- N75_cam:
- {}
- darwin/right_leg_pelvis_yaw:
- darwin/right_leg_pelvis_roll:
- darwin/right_leg_pelvis_pitch:
- darwin/right_leg_knee:
- darwin/right_leg_ankle_pitch:
- darwin/right_leg_ankle_roll:
- {}
- darwin/right_shoulder:
- darwin/right_arm_high:
- darwin/right_arm_low:
- {}
- obstacle_base_fence_link:
- {}
+ darwin/right_shoulder:
+ darwin/right_arm_high:
+ darwin/right_arm_low:
+ {}
Update Interval: 0
Value: true
- Alpha: 1
@@ -382,110 +298,10 @@ Visualization Manager:
Enabled: true
Links:
All Links Enabled: true
- E125:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- E175:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- E225:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- E25:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- E75:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
- N125:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- N175:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- N25:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- N75:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- S125:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- S175:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- S25:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- S75:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- W125:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- W175:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- W225:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- W25:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- W75:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- obstacle_base_fence_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- obstacle_base_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
Name: Environment
Robot Description: obstacles_environment
TF Prefix: ""
@@ -495,7 +311,7 @@ Visualization Manager:
Enabled: true
Global Options:
Background Color: 48; 48; 48
- Fixed Frame: obstacle_base_link
+ Fixed Frame: darwin/base_link
Frame Rate: 30
Name: root
Tools:
@@ -516,30 +332,30 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
- Distance: 1.84149
+ Distance: 4.15359
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
- X: 0.963161
- Y: -0.00493265
- Z: 0.233709
+ X: 0.625211
+ Y: -0.0443803
+ Z: 0.189017
Name: Current View
Near Clip Distance: 0.01
- Pitch: 0.664797
+ Pitch: 0.224797
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
- Yaw: 4.72545
+ Yaw: 5.70045
Saved: ~
Window Geometry:
Displays:
collapsed: false
- Height: 1056
+ Height: 936
Hide Left Dock: false
Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd0000000400000000000001ce00000396fc0200000007fb0000001200530065006c0065006300740069006f006e00000000280000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000004fb0000001e0054006f006f006c002000500072006f007000650072007400690065007301000000280000007e0000006400fffffffb0000000a0056006900650077007301000000ac00000312000000b000fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000031b000000a30000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004560000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd0000000400000000000001ce0000031efc0200000007fb0000001200530065006c0065006300740069006f006e00000000280000009b0000006400fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000031e000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000031efc0200000004fb0000001e0054006f006f006c002000500072006f007000650072007400690065007301000000280000006e0000006400fffffffb0000000a00560069006500770073010000009c000002aa000000b000fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073010000031b000000a30000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004bf0000003efc0100000002fb0000000800540069006d00650100000000000004bf000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000001d60000031e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -548,6 +364,6 @@ Window Geometry:
collapsed: false
Views:
collapsed: false
- Width: 1855
+ Width: 1215
X: 65
Y: 24
diff --git a/darwin_driver/CMakeLists.txt b/darwin_driver/CMakeLists.txt
index 4da699606951910068522cf5544124433e2f1c21..8d1df67063db34e2e90b79acabcbabc4bd0ed959 100644
--- a/darwin_driver/CMakeLists.txt
+++ b/darwin_driver/CMakeLists.txt
@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
-find_package(catkin REQUIRED COMPONENTS iri_base_driver std_msgs iri_action_server control_msgs trajectory_msgs iri_action_server iri_action_server humanoid_common_msgs geometry_msgs sensor_msgs)
+find_package(catkin REQUIRED COMPONENTS iri_base_driver darwin_smart_charger_client std_msgs iri_action_server control_msgs trajectory_msgs iri_action_server iri_action_server humanoid_common_msgs geometry_msgs sensor_msgs)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -16,7 +16,7 @@ find_package(catkin REQUIRED COMPONENTS iri_base_driver std_msgs iri_action_serv
# ********************************************************************
# find_package(<dependency> REQUIRED)
find_package(iriutils REQUIRED)
-find_package(darwin_robot REQUIRED)
+find_package(darwin_robot REQUIRED) #from Finddarwin_robot.cmake
# ********************************************************************
# Add topic, service and action definition here
@@ -62,7 +62,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
- CATKIN_DEPENDS iri_base_driver std_msgs iri_action_server control_msgs trajectory_msgs iri_action_server iri_action_server humanoid_common_msgs geometry_msgs sensor_msgs
+ CATKIN_DEPENDS iri_base_driver darwin_smart_charger_client std_msgs iri_action_server control_msgs trajectory_msgs iri_action_server iri_action_server humanoid_common_msgs geometry_msgs sensor_msgs
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
@@ -100,6 +100,7 @@ target_link_libraries(${PROJECT_NAME} ${darwin_robot_LIBRARY})
# Add message headers dependencies
# ********************************************************************
# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} darwin_smart_charger_client_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} std_msgs_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} control_msgs_generate_messages_cpp)
add_dependencies(${PROJECT_NAME} trajectory_msgs_generate_messages_cpp)
diff --git a/darwin_driver/cfg/DarwinDriver.cfg b/darwin_driver/cfg/DarwinDriver.cfg
index b8302288d1317fb0207b52de80788befb9440af5..6091edae554a77bdeafcc723bb9a44247501bdf3 100755
--- a/darwin_driver/cfg/DarwinDriver.cfg
+++ b/darwin_driver/cfg/DarwinDriver.cfg
@@ -43,5 +43,6 @@ gen = ParameterGenerator()
gen.add("darwin_serial", str_t, SensorLevels.RECONFIGURE_STOP, "Darwin serial device serial number", "A4008atn")
gen.add("darwin_id", int_t, SensorLevels.RECONFIGURE_STOP, "Darwin identifier", 2, 0, 254)
gen.add("darwin_baudrate", int_t, SensorLevels.RECONFIGURE_STOP, "Darwin serial baudrate", 1000000, 9600, 3000000)
+gen.add("cmd_vel_timeout", double_t, SensorLevels.RECONFIGURE_STOP, "Maximum time between cmd_vel messgaes",1.0, 0.1, 5)
exit(gen.generate(PACKAGE, "DarwinDriverDriver", "DarwinDriver"))
diff --git a/darwin_driver/include/darwin_driver.h b/darwin_driver/include/darwin_driver.h
index 5c8f7ab6980206c71951d0b173512c0d751bfebd..8fa3fc451740e74e5c636e65aa6d6867f225db8f 100644
--- a/darwin_driver/include/darwin_driver.h
+++ b/darwin_driver/include/darwin_driver.h
@@ -29,7 +29,7 @@
#include <darwin_driver/DarwinDriverConfig.h>
//include darwin_driver main library
-#include "darwin_robot.h"
+#include "darwin_robot.h" //de darwin_robot_driver
#include "darwin_robot_exceptions.h"
typedef struct
@@ -76,7 +76,7 @@ class DarwinDriver : public iri_base_driver::IriBaseDriver
{
private:
// private attributes and methods
- CDarwinRobot *darwin;
+ CDarwinRobot *darwin; //de darwin_robot_driver
TWalkParams walk_params;
public:
/**
@@ -197,6 +197,7 @@ class DarwinDriver : public iri_base_driver::IriBaseDriver
void walk_stop(void);
bool is_walking(void);
void walk(double step_x_m,double step_y_m,double step_rad);
+ double walk_get_timeout(void);
// joint motion interface
void joints_load(joints_grp_t group,std::vector<unsigned char> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels);
void joints_start(joints_grp_t group);
@@ -216,6 +217,18 @@ class DarwinDriver : public iri_base_driver::IriBaseDriver
bool head_is_tracking(void);
void head_set_new_target(double pan,double tilt);
void head_get_current_target(double *pan,double *tilt);
+
+ // smart charger interface
+ bool is_smart_charger_det_and_en(void);
+ void smart_charger_enable(void);
+ void smart_charger_disable(void);
+ void smart_charger_set_period(double period);
+ double smart_charger_get_period(void);
+ // unsigned char smart_charger_read_control(void);
+ TChargerData smart_charger_get_data(void);
+ void smart_charger_set_limit_current(double limit_current);
+ // unsigned char smart_charger_read_control(void);
+
/**
* \brief Destructor
*
diff --git a/darwin_driver/include/darwin_driver_node.h b/darwin_driver/include/darwin_driver_node.h
index 7611a2f107c269362bbf3891ea2c2a4c22d2d263..5057d9a8ff1da79164170437c04e9bae28cebfaa 100644
--- a/darwin_driver/include/darwin_driver_node.h
+++ b/darwin_driver/include/darwin_driver_node.h
@@ -29,6 +29,7 @@
#include "darwin_driver.h"
// [publisher subscriber headers]
+#include <darwin_smart_charger_client/smart_charger_data.h>
#include <humanoid_common_msgs/leds.h>
#include <humanoid_common_msgs/buttons.h>
#include <sensor_msgs/Imu.h>
@@ -37,6 +38,11 @@
#include <sensor_msgs/JointState.h>
// [service client headers]
+#include <darwin_smart_charger_client/set_config.h>
+#include <darwin_smart_charger_client/set_limit_current.h>
+#include <darwin_smart_charger_client/smart_charger_enable.h>
+#include <darwin_smart_charger_client/get_period.h>
+#include <darwin_smart_charger_client/set_period.h>
#include <humanoid_common_msgs/set_pid.h>
#include <humanoid_common_msgs/get_pid.h>
#include <humanoid_common_msgs/get_walk_params.h>
@@ -70,6 +76,9 @@ class DarwinDriverNode : public iri_base_driver::IriBaseNodeDriver<DarwinDriver>
{
private:
// [publisher attributes]
+ ros::Publisher smart_charger_data_publisher_;
+ darwin_smart_charger_client::smart_charger_data smart_charger_data_smart_charger_data_msg_;
+
ros::Publisher buttons_publisher_;
humanoid_common_msgs::buttons buttons_buttons_msg_;
@@ -101,6 +110,40 @@ class DarwinDriverNode : public iri_base_driver::IriBaseNodeDriver<DarwinDriver>
ros::Time walk_timeout;
// [service attributes]
+ ros::ServiceServer smart_charger_set_config_server_;
+ bool smart_charger_set_configCallback(darwin_smart_charger_client::set_config::Request &req, darwin_smart_charger_client::set_config::Response &res);
+ pthread_mutex_t smart_charger_set_config_mutex_;
+ void smart_charger_set_config_mutex_enter(void);
+ void smart_charger_set_config_mutex_exit(void);
+
+ ros::ServiceServer smart_charger_set_limit_current_server_;
+ bool smart_charger_set_limit_currentCallback(darwin_smart_charger_client::set_limit_current::Request &req, darwin_smart_charger_client::set_limit_current::Response &res);
+ pthread_mutex_t smart_charger_set_limit_current_mutex_;
+ void smart_charger_set_limit_current_mutex_enter(void);
+ void smart_charger_set_limit_current_mutex_exit(void);
+
+
+//Enable/Disable smart charger module
+ ros::ServiceServer smart_charger_enable_disable_server_;
+ bool smart_charger_enable_disableCallback(darwin_smart_charger_client::smart_charger_enable::Request &req, darwin_smart_charger_client::smart_charger_enable::Response &res);
+ pthread_mutex_t smart_charger_enable_disable_mutex_;
+ void smart_charger_enable_disable_mutex_enter(void);
+ void smart_charger_enable_disable_mutex_exit(void);
+
+//Get period of smart charger module
+ ros::ServiceServer smart_charger_get_period_server_;
+ bool smart_charger_get_periodCallback(darwin_smart_charger_client::get_period::Request &req, darwin_smart_charger_client::get_period::Response &res);
+ pthread_mutex_t smart_charger_get_period_mutex_;
+ void smart_charger_get_period_mutex_enter(void);
+ void smart_charger_get_period_mutex_exit(void);
+
+//Set period of smart charger module
+ ros::ServiceServer smart_charger_set_period_server_;
+ bool smart_charger_set_periodCallback(darwin_smart_charger_client::set_period::Request &req, darwin_smart_charger_client::set_period::Response &res);
+ pthread_mutex_t smart_charger_set_period_mutex_;
+ void smart_charger_set_period_mutex_enter(void);
+ void smart_charger_set_period_mutex_exit(void);
+
ros::ServiceServer set_tilt_pid_server_;
bool set_tilt_pidCallback(humanoid_common_msgs::set_pid::Request &req, humanoid_common_msgs::set_pid::Response &res);
pthread_mutex_t set_tilt_pid_mutex_;
diff --git a/darwin_driver/package.xml b/darwin_driver/package.xml
index b7b0ff0153fd121016bf14e4a77bfcec2cbbb790..8033b64e3b0826257d3e0f873309b46eface010c 100644
--- a/darwin_driver/package.xml
+++ b/darwin_driver/package.xml
@@ -41,6 +41,7 @@
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>iri_base_driver</build_depend>
+ <build_depend>darwin_smart_charger_client</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>control_msgs</build_depend>
<build_depend>trajectory_msgs</build_depend>
@@ -49,6 +50,7 @@
<build_depend>geometry_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<run_depend>iri_base_driver</run_depend>
+ <run_depend>darwin_smart_charger_client</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>control_msgs</run_depend>
<run_depend>trajectory_msgs</run_depend>
diff --git a/darwin_driver/src/darwin_driver.cpp b/darwin_driver/src/darwin_driver.cpp
index 08427a4366e64cf84abc21837a1429072003744d..57f86e396718df795096daf6e0aa89ae5a1950dd 100644
--- a/darwin_driver/src/darwin_driver.cpp
+++ b/darwin_driver/src/darwin_driver.cpp
@@ -106,7 +106,12 @@ bool DarwinDriver::startDriver(void)
}
else
ROS_INFO("Gyroscope not detected");
-
+ //Detected smart charger module
+ if(this->darwin->is_smart_charger_detected())
+ ROS_INFO("Smart charger detected");
+ else
+ ROS_INFO("Smart charger not detected");
+
return true;
}catch(CException &e){
ROS_WARN_STREAM(e.what());
@@ -442,6 +447,11 @@ void DarwinDriver::walk(double step_x_m,double step_y_m,double step_rad)
}
}
+double DarwinDriver::walk_get_timeout(void)
+{
+ return this->config_.cmd_vel_timeout;
+}
+
// joint motion interface
void DarwinDriver::joints_load(joints_grp_t group,std::vector<unsigned char> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels)
{
@@ -550,6 +560,43 @@ void DarwinDriver::head_get_current_target(double *pan,double *tilt)
this->darwin->head_get_current_target(pan,tilt);
}
+//smart charger interface
+bool DarwinDriver::is_smart_charger_det_and_en(void)
+{
+ if(this->darwin!=NULL)
+ return this->darwin->is_smart_charger_det_and_en();
+}
+void DarwinDriver::smart_charger_enable(void)
+{
+ if(this->darwin!=NULL)
+ this->darwin->smart_charger_enable();
+}
+void DarwinDriver::smart_charger_disable(void)
+{
+ if(this->darwin!=NULL)
+ this->darwin->smart_charger_disable();
+}
+void DarwinDriver::smart_charger_set_period(double period)
+{
+ if(this->darwin!=NULL)
+ this->darwin->smart_charger_set_period(period);
+}
+double DarwinDriver::smart_charger_get_period(void)
+{
+ if(this->darwin!=NULL)
+ return this->darwin->smart_charger_get_period();
+}
+TChargerData DarwinDriver::smart_charger_get_data(void)
+{
+ if(this->darwin!=NULL)
+ return this->darwin->smart_charger_get_data();
+}
+void DarwinDriver::smart_charger_set_limit_current(double limit_current)
+{
+ if(this->darwin!=NULL)
+ this->darwin->smart_charger_set_limit_current(limit_current);
+}
+
DarwinDriver::~DarwinDriver(void)
{
}
diff --git a/darwin_driver/src/darwin_driver_node.cpp b/darwin_driver/src/darwin_driver_node.cpp
index 53e8f4e71fe8aa92d7a2146c98495e56138f91f0..aa476b1332921b533d9d95eeb42b3f7a8a55de8a 100644
--- a/darwin_driver/src/darwin_driver_node.cpp
+++ b/darwin_driver/src/darwin_driver_node.cpp
@@ -41,7 +41,15 @@ DarwinDriverNode::DarwinDriverNode(ros::NodeHandle &nh) :
//init class attributes if necessary
//this->loop_rate_ = 2;//in [Hz]
+ /* this->darwin->imu_start();
+ if(this->darwin->imu_is_accel_detected())
+ ROS_INFO("Accelerometer detected");
+ else
+ ROS_INFO("Accelerometer not detected");
+ */
+
// [init publishers]
+ this->smart_charger_data_publisher_ = this->public_node_handle_.advertise<darwin_smart_charger_client::smart_charger_data>("smart_charger_data", 1);
this->buttons_publisher_ = this->public_node_handle_.advertise<humanoid_common_msgs::buttons>("buttons", 1);
this->raw_imu_publisher_ = this->public_node_handle_.advertise<sensor_msgs::Imu>("sensors/raw_imu", 1);
this->joint_states_publisher_ = this->public_node_handle_.advertise<sensor_msgs::JointState>("joint_states", 1);
@@ -58,6 +66,26 @@ DarwinDriverNode::DarwinDriverNode(ros::NodeHandle &nh) :
// [init services]
+//Configuration of smart charger module
+ this->smart_charger_set_config_server_ = this->public_node_handle_.advertiseService("smart_charger_set_config", &DarwinDriverNode::smart_charger_set_configCallback, this);
+ pthread_mutex_init(&this->smart_charger_set_config_mutex_,NULL);
+
+//Write smart charger limit current
+ this->smart_charger_set_limit_current_server_ = this->public_node_handle_.advertiseService("smart_charger_set_limit_current", &DarwinDriverNode::smart_charger_set_limit_currentCallback, this);
+ pthread_mutex_init(&this->smart_charger_set_limit_current_mutex_,NULL);
+
+//Enable/Disable smart charger module
+ this->smart_charger_enable_disable_server_ = this->public_node_handle_.advertiseService("smart_charger_enable_disable", &DarwinDriverNode::smart_charger_enable_disableCallback, this);
+ pthread_mutex_init(&this->smart_charger_enable_disable_mutex_,NULL);
+
+//Get period of smart charger module
+ this->smart_charger_get_period_server_ = this->public_node_handle_.advertiseService("smart_charger_get_period", &DarwinDriverNode::smart_charger_get_periodCallback, this);
+ pthread_mutex_init(&this->smart_charger_get_period_mutex_,NULL);
+
+//Set period of smart charger module
+ this->smart_charger_set_period_server_ = this->public_node_handle_.advertiseService("smart_charger_set_period", &DarwinDriverNode::smart_charger_set_periodCallback, this);
+ pthread_mutex_init(&this->smart_charger_set_period_mutex_,NULL);
+
this->set_tilt_pid_server_ = this->public_node_handle_.advertiseService("robot/set_tilt_pid", &DarwinDriverNode::set_tilt_pidCallback, this);
pthread_mutex_init(&this->set_tilt_pid_mutex_,NULL);
@@ -123,13 +151,17 @@ void DarwinDriverNode::mainNodeThread(void)
static ros::Time last_time=ros::Time(0,0);
ros::Time current_time=ros::Time::now();
std::vector<double> current_angles;
-
+ TChargerData smart_charger_data;
+
//lock access to driver if necessary
this->driver_.lock();
// [fill msg Header if necessary]
// [fill msg structures]
+ // Initialize the topic message structure
+ //this->smart_charger_data_smart_charger_data_msg_.data = my_var;
+
// Initialize the topic message structure
//this->raw_imu_Imu_msg_.data = my_var;
@@ -166,7 +198,7 @@ void DarwinDriverNode::mainNodeThread(void)
this->joint_states_JointState_msg_.header.stamp=ros::Time::now();
this->joint_states_publisher_.publish(this->joint_states_JointState_msg_);
- if(this->driver_.is_walking() && ((current_time-this->walk_timeout).toSec()>1.0))
+ if(this->driver_.is_walking() && ((current_time-this->walk_timeout).toSec()>this->driver_.walk_get_timeout()))
this->driver_.walk_stop();
// publish button info when a button is pressed
@@ -185,8 +217,30 @@ void DarwinDriverNode::mainNodeThread(void)
if(this->buttons_buttons_msg_.name.size()>0)
{
this->buttons_buttons_msg_.header.stamp=ros::Time::now();
- this->buttons_publisher_.publish(this->buttons_buttons_msg_);
- }
+ this->buttons_publisher_.publish(this->buttons_buttons_msg_); //Publicar por el topico de buttons el mensaje "buttons_buttons_msg_"
+ }
+
+ //publish smart charger data when is detected and enabled
+ if(this->driver_.is_smart_charger_det_and_en())
+ {
+ //ROS_INFO("Publishing smart charger data!");
+ smart_charger_data=this->driver_.smart_charger_get_data();
+ this->smart_charger_data_smart_charger_data_msg_.avg_time_empty=smart_charger_data.avg_time_empty;
+ this->smart_charger_data_smart_charger_data_msg_.avg_time_full=smart_charger_data.avg_time_full;
+ if(smart_charger_data.bat_status==0)
+ this->smart_charger_data_smart_charger_data_msg_.batt_status="Disconnected";
+ else if(smart_charger_data.bat_status==192)
+ this->smart_charger_data_smart_charger_data_msg_.batt_status="Connected";
+ else if(smart_charger_data.bat_status==128)
+ this->smart_charger_data_smart_charger_data_msg_.batt_status="Connected and charging";
+ else
+ this->smart_charger_data_smart_charger_data_msg_.batt_status="Unknown";
+
+ this->smart_charger_data_smart_charger_data_msg_.header.stamp=ros::Time::now();
+ this->smart_charger_data_publisher_.publish(this->smart_charger_data_smart_charger_data_msg_);
+ }
+
+
}catch(CException &e){
ROS_WARN_STREAM(e.what());
}
@@ -320,6 +374,267 @@ void DarwinDriverNode::cmd_vel_mutex_exit(void)
/* [service callbacks] */
+bool DarwinDriverNode::smart_charger_set_configCallback(darwin_smart_charger_client::set_config::Request &req, darwin_smart_charger_client::set_config::Response &res)
+{
+ ROS_INFO("DarwinDriverNode::smart_charger_set_configCallback: New Request Received!");
+
+ try{
+ //use appropiate mutex to shared variables if necessary
+ this->driver_.lock();
+ this->smart_charger_set_config_mutex_enter();
+
+ ROS_INFO("DarwinDriverNode::smart_charger_set_configCallback: Processing New Request!");
+
+//Enable smart charger module
+ if(req.enable){
+ ROS_INFO("DarwinDriverNode::smart_charger_set_configCallback: Enabling smart charger module");
+ this->driver_.smart_charger_enable();
+ res.success_en=true;
+ }
+//Disable smart charger module
+ if(req.disable && !req.enable){
+ ROS_INFO("DarwinDriverNode::smart_charger_set_configCallback: Disable smart charger module");
+ this->driver_.smart_charger_disable();
+ res.success_dis=true;
+ }
+
+//Set period of smart charger module
+ if(req.set_period){
+ if(req.period_value > 1.3 && req.period_value < 1.7)
+ {
+ ROS_INFO("DarwinDriverNode::smart_charger_set_configCallback: Setting period!");
+ this->driver_.smart_charger_set_period(req.period_value);
+ res.success_per=true;
+ }
+ else
+ {
+ ROS_INFO("DarwinDriverNode::smart_charger_set_configCallback: Invalid period value");
+ res.success_per=false;
+ }
+ }
+
+//Set limit current
+ if(req.set_limit_current)
+ {
+ ROS_INFO("DarwinDriverNode::smart_charger_set_configCallback: Setting limit current");
+ this->driver_.smart_charger_set_limit_current(req.current_value);
+ res.success_cur=true;
+ }
+
+ //unlock previously blocked shared variables
+ this->smart_charger_set_config_mutex_exit();
+ this->driver_.unlock();
+ }catch(CException &e){
+ ROS_WARN_STREAM(e.what());
+ res.success_en=false;
+ res.success_dis=false;
+ res.success_per=false;
+ res.success_cur=false;
+ //unlock previously blocked shared variables
+ this->smart_charger_set_config_mutex_exit();
+ this->driver_.unlock();
+ }
+
+ return true;
+}
+
+void DarwinDriverNode::smart_charger_set_config_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->smart_charger_set_config_mutex_);
+}
+
+void DarwinDriverNode::smart_charger_set_config_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->smart_charger_set_config_mutex_);
+}
+
+bool DarwinDriverNode::smart_charger_set_limit_currentCallback(darwin_smart_charger_client::set_limit_current::Request &req, darwin_smart_charger_client::set_limit_current::Response &res)
+{
+ ROS_INFO("DarwinDriverNode::smart_charger_set_limit_currentCallback: New Request Received!");
+
+ //use appropiate mutex to shared variables if necessary
+ //this->driver_.lock();
+ //this->smart_charger_set_limit_current_mutex_enter();
+
+ //ROS_INFO("DarwinDriverNode::smart_charger_set_limit_currentCallback: Processing New Request!");
+ //do operations with req and output on res
+ //res.data2 = req.data1 + my_var;
+
+ //unlock previously blocked shared variables
+ //this->smart_charger_set_limit_current_mutex_exit();
+ //this->driver_.unlock();
+
+ try{
+ this->driver_.lock();
+ this->smart_charger_set_limit_current_mutex_enter();
+
+ if(req.set_limit_current)
+ {
+ ROS_INFO("DarwinDriverNode::smart_charger_set_limit_currentCallback: Set limit current");
+ this->driver_.smart_charger_set_limit_current(req.limit_current);
+ res.success=true;
+ }
+
+ //unlock previously blocked shared variables
+ this->smart_charger_set_limit_current_mutex_exit();
+ this->driver_.unlock();
+
+ }catch(CException &e){
+ ROS_WARN_STREAM(e.what());
+ res.success=false;
+ this->smart_charger_set_limit_current_mutex_exit();
+ this->driver_.unlock();
+ }
+
+ return true;
+}
+
+void DarwinDriverNode::smart_charger_set_limit_current_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->smart_charger_set_limit_current_mutex_);
+}
+
+void DarwinDriverNode::smart_charger_set_limit_current_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->smart_charger_set_limit_current_mutex_);
+}
+
+bool DarwinDriverNode::smart_charger_enable_disableCallback(darwin_smart_charger_client::smart_charger_enable::Request &req, darwin_smart_charger_client::smart_charger_enable::Response &res)
+{
+ ROS_INFO("DarwinDriverNode::smart_charger_enable_disableCallback: New Request Received!");
+
+ //use appropiate mutex to shared variables if necessary
+ try{
+ this->driver_.lock();
+ this->smart_charger_enable_disable_mutex_enter();
+
+ if(req.enable){
+ ROS_INFO("DarwinDriverNode::smart_charger_enable_disableCallback: Enable smart charger module");
+ this->driver_.smart_charger_enable();
+ }else{
+ ROS_INFO("DarwinDriverNode::smart_charger_enable_disableCallback: Disable smart charger module");
+ this->driver_.smart_charger_disable();
+ }
+ res.success=true;
+
+ //unlock previously blocked shared variables
+ this->smart_charger_enable_disable_mutex_exit();
+ this->driver_.unlock();
+
+ }catch(CException &e){
+ ROS_WARN_STREAM(e.what());
+ res.success=false;
+ this->smart_charger_enable_disable_mutex_exit();
+ this->driver_.unlock();
+ }
+
+ return true;
+}
+
+void DarwinDriverNode::smart_charger_enable_disable_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->smart_charger_enable_disable_mutex_);
+}
+
+void DarwinDriverNode::smart_charger_enable_disable_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->smart_charger_enable_disable_mutex_);
+}
+
+bool DarwinDriverNode::smart_charger_get_periodCallback(darwin_smart_charger_client::get_period::Request &req, darwin_smart_charger_client::get_period::Response &res)
+{
+ ROS_INFO("DarwinDriverNode::smart_charger_get_periodCallback: New Request Received!");
+
+ //use appropiate mutex to shared variables if necessary
+ try{
+ this->driver_.lock();
+ this->smart_charger_get_period_mutex_enter();
+
+ //ROS_INFO("DarwinDriverNode::smart_charger_get_periodCallback: Processing New Request!");
+ //do operations with req and output on res
+ //res.data2 = req.data1 + my_var;
+ //this->driver_.smart_charger_get_period((double *)&res.period);
+ res.period_value = this->driver_.smart_charger_get_period();
+ res.success=true;
+
+ //unlock previously blocked shared variables
+ this->smart_charger_get_period_mutex_exit();
+ this->driver_.unlock();
+ }catch(CException &e){
+ ROS_WARN_STREAM(e.what());
+ res.success=false;
+ this->smart_charger_get_period_mutex_exit();
+ this->driver_.unlock();
+ }
+
+ return true;
+}
+
+void DarwinDriverNode::smart_charger_get_period_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->smart_charger_get_period_mutex_);
+}
+
+void DarwinDriverNode::smart_charger_get_period_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->smart_charger_get_period_mutex_);
+}
+
+bool DarwinDriverNode::smart_charger_set_periodCallback(darwin_smart_charger_client::set_period::Request &req, darwin_smart_charger_client::set_period::Response &res)
+{
+ ROS_INFO("DarwinDriverNode::smart_charger_set_periodCallback: New Request Received!");
+
+ //use appropiate mutex to shared variables if necessary
+ //this->driver_.lock();
+ //this->smart_charger_set_period_mutex_enter();
+
+ //ROS_INFO("DarwinDriverNode::smart_charger_set_periodCallback: Processing New Request!");
+ //do operations with req and output on res
+ //res.data2 = req.data1 + my_var;
+
+ //unlock previously blocked shared variables
+ //this->smart_charger_set_period_mutex_exit();
+ //this->driver_.unlock();
+
+ try{
+ this->driver_.lock();
+ this->smart_charger_set_period_mutex_enter();
+
+ if(req.period > 1.3 && req.period < 1.7)
+ {
+ this->driver_.smart_charger_set_period(req.period);
+ res.ret=true;
+ }
+ else
+ {
+ ROS_INFO("DarwinDriverNode::smart_charger_set_periodCallback: Invalid period value");
+ res.ret=false;
+ }
+
+ //unlock previously blocked shared variables
+ this->smart_charger_set_period_mutex_exit();
+ this->driver_.unlock();
+ }catch(CException &e){
+ ROS_WARN_STREAM(e.what());
+ res.ret=false;
+ this->smart_charger_set_period_mutex_exit();
+ this->driver_.unlock();
+ }
+
+
+ return true;
+}
+
+void DarwinDriverNode::smart_charger_set_period_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->smart_charger_set_period_mutex_);
+}
+
+void DarwinDriverNode::smart_charger_set_period_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->smart_charger_set_period_mutex_);
+}
+
bool DarwinDriverNode::set_tilt_pidCallback(humanoid_common_msgs::set_pid::Request &req, humanoid_common_msgs::set_pid::Response &res)
{
ROS_INFO("DarwinDriverNode::set_tilt_pidCallback: New Request Received!");
@@ -951,6 +1266,11 @@ void DarwinDriverNode::reconfigureNodeHook(int level)
DarwinDriverNode::~DarwinDriverNode(void)
{
// [free dynamic memory]
+ pthread_mutex_destroy(&this->smart_charger_set_config_mutex_);
+ pthread_mutex_destroy(&this->smart_charger_set_limit_current_mutex_);
+ pthread_mutex_destroy(&this->smart_charger_enable_disable_mutex_);
+ pthread_mutex_destroy(&this->smart_charger_get_period_mutex_);
+ pthread_mutex_destroy(&this->smart_charger_set_period_mutex_);
pthread_mutex_destroy(&this->leds_mutex_);
pthread_mutex_destroy(&this->set_tilt_pid_mutex_);
pthread_mutex_destroy(&this->set_pan_pid_mutex_);
diff --git a/darwin_odometry/include/darwin_odometry_alg.h b/darwin_odometry/include/darwin_odometry_alg.h
index 092b0c1d50d78dc021e3aa64be874eaba0637b04..870b0176029bc55f315e2815b175bd94b67974dd 100644
--- a/darwin_odometry/include/darwin_odometry_alg.h
+++ b/darwin_odometry/include/darwin_odometry_alg.h
@@ -61,6 +61,7 @@ class DarwinOdometryAlgorithm
bool right_leg_fk_ready;
public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief define config type
*
diff --git a/darwin_robot/package.xml b/darwin_robot/package.xml
index 405769014791a7ac35bd615d9665b74693fa42b1..2ad889a113567eeb31e8009da2bbfba17d263e36 100644
--- a/darwin_robot/package.xml
+++ b/darwin_robot/package.xml
@@ -47,7 +47,10 @@
<run_depend>darwin_odometry</run_depend>
<run_depend>darwin_buttons</run_depend>
<run_depend>darwin_leds_client</run_depend>
-<!-- <run_depend>darwin_robot_driver</run_depend>-->
+ <run_depend>darwin_smart_charger_client</run_depend>
+ <run_depend>sm_qr_search</run_depend>
+ <run_depend>track_charge_station</run_depend>
+ <!-- <run_depend>darwin_robot_driver</run_depend>-->
<!-- <run_depend>darwin_arm_kinematics</run_depend>-->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
diff --git a/darwin_smart_charger_client/CMakeLists.txt b/darwin_smart_charger_client/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..984e10b92d98eef5f091f3ee0411f84852b012b4
--- /dev/null
+++ b/darwin_smart_charger_client/CMakeLists.txt
@@ -0,0 +1,106 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(darwin_smart_charger_client)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED)
+# ********************************************************************
+# Add catkin additional components here
+# ********************************************************************
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm std_msgs message_generation)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+# ********************************************************************
+# Add system and labrobotica dependencies here
+# ********************************************************************
+# find_package(<dependency> REQUIRED)
+
+# ********************************************************************
+# Add topic, service and action definition here
+# ********************************************************************
+## Generate messages in the 'msg' folder
+ add_message_files(
+ FILES
+ smart_charger_data.msg
+# Message2.msg
+ )
+
+## Generate services in the 'srv' folder
+ add_service_files(
+# DIRECTORY srv
+ FILES
+ set_config.srv
+ set_period.srv
+ get_period.srv
+ smart_charger_enable.srv
+ set_limit_current.srv
+ )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+ generate_messages(
+ DEPENDENCIES
+ std_msgs # Or other packages containing msgs
+ )
+
+# ********************************************************************
+# Add the dynamic reconfigure file
+# ********************************************************************
+generate_dynamic_reconfigure_options(cfg/DarwinSmartChargerClient.cfg)
+
+# ********************************************************************
+# Add run time dependencies here
+# ********************************************************************
+catkin_package(
+# INCLUDE_DIRS
+# LIBRARIES
+# ********************************************************************
+# Add ROS and IRI ROS run time dependencies
+# ********************************************************************
+ #std_msgs, message_generation when added the client
+ CATKIN_DEPENDS iri_base_algorithm std_msgs #message_generation
+# ********************************************************************
+# Add system and labrobotica run time dependencies here
+# ********************************************************************
+# DEPENDS
+)
+
+###########
+## Build ##
+###########
+
+# ********************************************************************
+# Add the include directories
+# ********************************************************************
+include_directories(include)
+include_directories(${catkin_INCLUDE_DIRS})
+# include_directories(${<dependency>_INCLUDE_DIR})
+
+## Declare a cpp library
+# add_library(${PROJECT_NAME} <list of source files>)
+
+## Declare a cpp executable
+add_executable(${PROJECT_NAME} src/darwin_smart_charger_client_alg.cpp src/darwin_smart_charger_client_alg_node.cpp)
+
+# ********************************************************************
+# Add the libraries
+# ********************************************************************
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+# target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
+
+# ********************************************************************
+# Add message headers dependencies
+# ********************************************************************
+# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} darwin_smart_charger_client_generate_messages_cpp)
+# ********************************************************************
+# Add dynamic reconfigure dependencies
+# ********************************************************************
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
diff --git a/darwin_smart_charger_client/cfg/DarwinSmartChargerClient.cfg b/darwin_smart_charger_client/cfg/DarwinSmartChargerClient.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..c240299100735677381b1777f7790b4aaa1806ae
--- /dev/null
+++ b/darwin_smart_charger_client/cfg/DarwinSmartChargerClient.cfg
@@ -0,0 +1,51 @@
+#! /usr/bin/env python
+#* All rights reserved.
+#*
+#* Redistribution and use in source and binary forms, with or without
+#* modification, are permitted provided that the following conditions
+#* are met:
+#*
+#* * Redistributions of source code must retain the above copyright
+#* notice, this list of conditions and the following disclaimer.
+#* * Redistributions in binary form must reproduce the above
+#* copyright notice, this list of conditions and the following
+#* disclaimer in the documentation and/or other materials provided
+#* with the distribution.
+#* * Neither the name of the Willow Garage nor the names of its
+#* contributors may be used to endorse or promote products derived
+#* from this software without specific prior written permission.
+#*
+#* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#* POSSIBILITY OF SUCH DAMAGE.
+#***********************************************************
+
+# Author:
+
+PACKAGE='darwin_smart_charger_client'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# Name Type Reconfiguration level Description Default Min Max
+gen.add("enable", bool_t, 0, "Enable/Disable smart charger module", False)
+gen.add("disable", bool_t, 0, "Disable smart charger module", False)
+gen.add("set_period", bool_t, 0, "Set period smart charger module", False)
+gen.add("period_value", double_t, 0, "Period value", 1.5, 1.4, 1.6)
+gen.add("get_period", bool_t, 0, "Get period smart charger module", False)
+gen.add("set_limit_current", bool_t, 0, "Set limit current smart charger module", False)
+gen.add("current_value", double_t, 0, "Limit current value", 0.512, 0.512, 1.024)
+
+#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
+
+exit(gen.generate(PACKAGE, "DarwinSmartChargerClientAlgorithm", "DarwinSmartChargerClient"))
diff --git a/darwin_smart_charger_client/darwin_smart_charger_client.launch b/darwin_smart_charger_client/darwin_smart_charger_client.launch
new file mode 100644
index 0000000000000000000000000000000000000000..0e2c661f6815deaaff51c3ee968a54312b488629
--- /dev/null
+++ b/darwin_smart_charger_client/darwin_smart_charger_client.launch
@@ -0,0 +1,54 @@
+<launch>
+
+ <arg name="reconfigure"
+ default="true"/>
+
+ <node pkg="darwin_smart_charger_client"
+ type="darwin_smart_charger_client"
+ name="darwin_smart_charger_client"
+ output="screen">
+ <param name="enable"
+ type="bool"
+ value="false"/>
+ <param name="disable"
+ type="bool"
+ value="false"/>
+ <param name="set_period"
+ type="bool"
+ value="false"/>
+ <param name="period_value"
+ type="double"
+ value="1.5"/>
+ <param name="get_period"
+ type="bool"
+ value="false"/>
+ <param name="set_limit_current"
+ type="bool"
+ value="false"/>
+ <param name="current_value"
+ type="double"
+ value="0.5"/>
+ <remap from="/darwin_smart_charger_client/smart_charger_set_config"
+ to="/darwin/smart_charger_set_config"/>
+ <remap from="/darwin_smart_charger_client/smart_charger_get_period"
+ to="/darwin/smart_charger_get_period"/>
+ <remap from="/darwin_smart_charger_client/smart_charger_data"
+ to="/darwin/smart_charger_data"/>
+ </node>
+
+ <node if="$(arg reconfigure)"
+ pkg="rqt_reconfigure"
+ type="rqt_reconfigure"
+ name="rqt_reconfigure"/>
+
+ <node name="darwin"
+ pkg="darwin_driver"
+ type="darwin_driver"
+ output="screen">
+ <param name="darwin_serial" value="A603LOBS"/>
+ <param name="darwin_baudrate" value="1000000"/>
+ <param name="darwin_id" value="2"/>
+ </node>
+
+</launch>
+
diff --git a/darwin_smart_charger_client/include/darwin_smart_charger_client_alg.h b/darwin_smart_charger_client/include/darwin_smart_charger_client_alg.h
new file mode 100644
index 0000000000000000000000000000000000000000..f7d0f45ced487c1fbde19e139c8ee245250da331
--- /dev/null
+++ b/darwin_smart_charger_client/include/darwin_smart_charger_client_alg.h
@@ -0,0 +1,131 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+// IMPORTANT NOTE: This code has been generated through a script from the
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _darwin_smart_charger_client_alg_h_
+#define _darwin_smart_charger_client_alg_h_
+
+#include <darwin_smart_charger_client/DarwinSmartChargerClientConfig.h>
+
+//include darwin_smart_charger_client_alg main library
+
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ *
+ */
+class DarwinSmartChargerClientAlgorithm
+{
+ protected:
+ /**
+ * \brief define config type
+ *
+ * Define a Config type with the DarwinSmartChargerClientConfig. All driver implementations
+ * will then use the same variable type Config.
+ */
+ pthread_mutex_t access_;
+
+ // private attributes and methods
+
+ public:
+ /**
+ * \brief define config type
+ *
+ * Define a Config type with the DarwinSmartChargerClientConfig. All driver implementations
+ * will then use the same variable type Config.
+ */
+ typedef darwin_smart_charger_client::DarwinSmartChargerClientConfig Config;
+
+ /**
+ * \brief config variable
+ *
+ * This variable has all the driver parameters defined in the cfg config file.
+ * Is updated everytime function config_update() is called.
+ */
+ Config config_;
+
+ /**
+ * \brief constructor
+ *
+ * In this constructor parameters related to the specific driver can be
+ * initalized. Those parameters can be also set in the openDriver() function.
+ * Attributes from the main node driver class IriBaseDriver such as loop_rate,
+ * may be also overload here.
+ */
+ DarwinSmartChargerClientAlgorithm(void);
+
+ /**
+ * \brief Lock Algorithm
+ *
+ * Locks access to the Algorithm class
+ */
+ void lock(void) { pthread_mutex_lock(&this->access_); };
+
+ /**
+ * \brief Unlock Algorithm
+ *
+ * Unlocks access to the Algorithm class
+ */
+ void unlock(void) { pthread_mutex_unlock(&this->access_); };
+
+ /**
+ * \brief Tries Access to Algorithm
+ *
+ * Tries access to Algorithm
+ *
+ * \return true if the lock was adquired, false otherwise
+ */
+ bool try_enter(void)
+ {
+ if(pthread_mutex_trylock(&this->access_)==0)
+ return true;
+ else
+ return false;
+ };
+
+ /**
+ * \brief config update
+ *
+ * In this function the driver parameters must be updated with the input
+ * config variable. Then the new configuration state will be stored in the
+ * Config attribute.
+ *
+ * \param new_cfg the new driver configuration state
+ *
+ * \param level level in which the update is taken place
+ */
+ void config_update(Config& config, uint32_t level=0);
+
+ // here define all darwin_smart_charger_client_alg interface methods to retrieve and set
+ // the driver parameters
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor is called when the object is about to be destroyed.
+ *
+ */
+ ~DarwinSmartChargerClientAlgorithm(void);
+};
+
+#endif
diff --git a/darwin_smart_charger_client/include/darwin_smart_charger_client_alg_node.h b/darwin_smart_charger_client/include/darwin_smart_charger_client_alg_node.h
new file mode 100644
index 0000000000000000000000000000000000000000..3d7c88b4684db1f1cad7d22f5943047dfea6c0fd
--- /dev/null
+++ b/darwin_smart_charger_client/include/darwin_smart_charger_client_alg_node.h
@@ -0,0 +1,142 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+// IMPORTANT NOTE: This code has been generated through a script from the
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _darwin_smart_charger_client_alg_node_h_
+#define _darwin_smart_charger_client_alg_node_h_
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+#include "darwin_smart_charger_client_alg.h"
+
+// [publisher subscriber headers]
+#include <darwin_smart_charger_client/smart_charger_data.h>
+
+// [service client headers]
+#include <darwin_smart_charger_client/set_config.h>
+#include <darwin_smart_charger_client/get_period.h>
+#include <darwin_smart_charger_client/set_period.h> //cuando he añadido el cliente set_period
+
+// [action server client headers]
+
+/**
+ * \brief IRI ROS Specific Algorithm Class
+ *
+ */
+class DarwinSmartChargerClientAlgNode : public algorithm_base::IriBaseAlgorithm<DarwinSmartChargerClientAlgorithm>
+{
+ private:
+ // [publisher attributes]
+
+ // [subscriber attributes]
+ ros::Subscriber smart_charger_data_subscriber_;
+ void smart_charger_data_callback(const darwin_smart_charger_client::smart_charger_data::ConstPtr& msg);
+ pthread_mutex_t smart_charger_data_mutex_;
+ void smart_charger_data_mutex_enter(void);
+ void smart_charger_data_mutex_exit(void);
+
+
+ // [service attributes]
+
+ // [client attributes]
+ ros::ServiceClient smart_charger_set_config_client_;
+ darwin_smart_charger_client::set_config smart_charger_set_config_srv_;
+
+ ros::ServiceClient smart_charger_get_period_client_;
+ darwin_smart_charger_client::get_period smart_charger_get_period_srv_;
+
+//cuando he añadido el cliente set_perio - defined the service's client object and the variable where is saved the input and output of the service.
+ ros::ServiceClient smart_charger_set_period_client_;
+ darwin_smart_charger_client::set_period smart_charger_set_period_srv_;
+
+
+ // [action server attributes]
+
+ // [action client attributes]
+
+ /**
+ * \brief config variable
+ *
+ * This variable has all the driver parameters defined in the cfg config file.
+ * Is updated everytime function config_update() is called.
+ */
+ Config config_;
+ public:
+ /**
+ * \brief Constructor
+ *
+ * This constructor initializes specific class attributes and all ROS
+ * communications variables to enable message exchange.
+ */
+ DarwinSmartChargerClientAlgNode(void);
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor frees all necessary dynamic memory allocated within this
+ * this class.
+ */
+ ~DarwinSmartChargerClientAlgNode(void);
+
+ protected:
+ /**
+ * \brief main node thread
+ *
+ * This is the main thread node function. Code written here will be executed
+ * in every node loop while the algorithm is on running state. Loop frequency
+ * can be tuned by modifying loop_rate attribute.
+ *
+ * Here data related to the process loop or to ROS topics (mainly data structs
+ * related to the MSG and SRV files) must be updated. ROS publisher objects
+ * must publish their data in this process. ROS client servers may also
+ * request data to the corresponding server topics.
+ */
+ void mainNodeThread(void);
+
+ /**
+ * \brief dynamic reconfigure server callback
+ *
+ * This method is called whenever a new configuration is received through
+ * the dynamic reconfigure. The derivated generic algorithm class must
+ * implement it.
+ *
+ * \param config an object with new configuration from all algorithm
+ * parameters defined in the config file.
+ * \param level integer referring the level in which the configuration
+ * has been changed.
+ */
+ void node_config_update(Config &config, uint32_t level);
+
+ /**
+ * \brief node add diagnostics
+ *
+ * In this abstract function additional ROS diagnostics applied to the
+ * specific algorithms may be added.
+ */
+ void addNodeDiagnostics(void);
+
+ // [diagnostic functions]
+
+ // [test functions]
+};
+
+#endif
diff --git a/darwin_smart_charger_client/msg/smart_charger_data.msg b/darwin_smart_charger_client/msg/smart_charger_data.msg
new file mode 100644
index 0000000000000000000000000000000000000000..95d386bc7620130d42c2d693035543e8ce0e4e41
--- /dev/null
+++ b/darwin_smart_charger_client/msg/smart_charger_data.msg
@@ -0,0 +1,5 @@
+Header header
+uint16 avg_time_empty
+uint16 avg_time_full
+string batt_status
+
diff --git a/darwin_smart_charger_client/package.xml b/darwin_smart_charger_client/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..6939dc3a2d5ad26124ec95fa61fc219625e69e1e
--- /dev/null
+++ b/darwin_smart_charger_client/package.xml
@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<package>
+ <name>darwin_smart_charger_client</name>
+ <version>0.0.0</version>
+ <description>The darwin_smart_charger_client package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="igarcia@todo.todo">igarcia</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>LGPL</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/darwin_smart_charger_client</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!--std_msgs and message_generation when added client -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>iri_base_algorithm</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <run_depend>iri_base_algorithm</run_depend>
+ <run_depend>std_msgs</run_depend>
+ <!-- <run_depend>message_generation</run_depend>
+ <build_depend>message_generation</build_depend>
+ -->
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/darwin_smart_charger_client/src/darwin_smart_charger_client_alg.cpp b/darwin_smart_charger_client/src/darwin_smart_charger_client_alg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..10c81af813ec5fd88e5022500bf0597366857cda
--- /dev/null
+++ b/darwin_smart_charger_client/src/darwin_smart_charger_client_alg.cpp
@@ -0,0 +1,23 @@
+#include "darwin_smart_charger_client_alg.h"
+
+DarwinSmartChargerClientAlgorithm::DarwinSmartChargerClientAlgorithm(void)
+{
+ pthread_mutex_init(&this->access_,NULL);
+}
+
+DarwinSmartChargerClientAlgorithm::~DarwinSmartChargerClientAlgorithm(void)
+{
+ pthread_mutex_destroy(&this->access_);
+}
+
+void DarwinSmartChargerClientAlgorithm::config_update(Config& config, uint32_t level)
+{
+ this->lock();
+
+ // save the current configuration
+ this->config_=config;
+
+ this->unlock();
+}
+
+// DarwinSmartChargerClientAlgorithm Public API
diff --git a/darwin_smart_charger_client/src/darwin_smart_charger_client_alg_node.cpp b/darwin_smart_charger_client/src/darwin_smart_charger_client_alg_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0a9f213435c93761876c7eef5b08c0706c20aca2
--- /dev/null
+++ b/darwin_smart_charger_client/src/darwin_smart_charger_client_alg_node.cpp
@@ -0,0 +1,257 @@
+#include "darwin_smart_charger_client_alg_node.h"
+
+DarwinSmartChargerClientAlgNode::DarwinSmartChargerClientAlgNode(void) :
+ algorithm_base::IriBaseAlgorithm<DarwinSmartChargerClientAlgorithm>()
+{
+ //init class attributes if necessary
+ //this->loop_rate_ = 2;//in [Hz]
+
+ // [init publishers]
+
+ // [init subscribers]
+ this->smart_charger_data_subscriber_ = this->public_node_handle_.subscribe("smart_charger_data", 1, &DarwinSmartChargerClientAlgNode::smart_charger_data_callback, this);
+ pthread_mutex_init(&this->smart_charger_data_mutex_,NULL);
+
+
+ // [init services]
+
+ // [init clients]
+ smart_charger_set_config_client_ = this->public_node_handle_.serviceClient<darwin_smart_charger_client::set_config>("smart_charger_set_config");
+
+ smart_charger_get_period_client_ = this->public_node_handle_.serviceClient<darwin_smart_charger_client::get_period>("smart_charger_get_period"); //cliente get_period
+
+ smart_charger_set_period_client_ = this->public_node_handle_.serviceClient<darwin_smart_charger_client::set_period>("smart_charger_set_period"); //cliente set_period
+
+
+ // [init action servers]
+
+ // [init action clients]
+}
+
+DarwinSmartChargerClientAlgNode::~DarwinSmartChargerClientAlgNode(void)
+{
+ // [free dynamic memory]
+ pthread_mutex_destroy(&this->smart_charger_data_mutex_);
+}
+
+void DarwinSmartChargerClientAlgNode::mainNodeThread(void)
+{
+ // [fill msg structures]
+
+ // [fill srv structure and make request to the server]
+ //smart_charger_set_config_srv_.request.data = my_var;
+ //ROS_INFO("DarwinSmartChargerClientAlgNode:: Sending New Request!");
+ //if (smart_charger_set_config_client_.call(smart_charger_set_config_srv_))
+ //{
+ //ROS_INFO("DarwinSmartChargerClientAlgNode:: Response: %s", smart_charger_set_config_srv_.response.result);
+ //}
+ //else
+ //{
+ //ROS_INFO("DarwinSmartChargerClientAlgNode:: Failed to Call Server on topic smart_charger_set_config ");
+ //}
+
+
+
+//CLIENTE GET PERIOD
+ //smart_charger_get_period_srv_.request.data = my_var;
+ //ROS_INFO("DarwinSmartChargerClientAlgNode:: Sending New Request!");
+/* if(config.get_period){
+ if (smart_charger_get_period_client_.call(smart_charger_get_period_srv_))
+ {
+ ROS_INFO("DarwinSmartChargerClientAlgNode:: Response: %s", smart_charger_get_period_srv_.response.period);
+ }
+ else
+ {
+ ROS_INFO("DarwinSmartChargerClientAlgNode:: Failed to Call Server on topic smart_charger_get_period ");
+ }
+ config.get_period=false;
+ }
+*/
+ //smart_charger_set_period_srv_.request.data = my_var;
+ //ROS_INFO("DarwinSmartChargerClientAlgNode:: Sending New Request!");
+ //if (smart_charger_set_period_client_.call(smart_charger_set_period_srv_))
+ //{
+ //ROS_INFO("DarwinSmartChargerClientAlgNode:: Response: %s", smart_charger_set_period_srv_.response.result);
+ //}
+ //else
+ //{
+ //ROS_INFO("DarwinSmartChargerClientAlgNode:: Failed to Call Server on topic smart_charger_set_period ");
+ //}
+
+
+//CLIENTE SET_PERIOD
+ //set_period_srv_.request.data = my_var;
+ //ROS_INFO("DarwinSmartChargerClientAlgNode:: Sending New Request!");
+ //if (set_period_client_.call(set_period_srv_))
+ //{
+ //ROS_INFO("DarwinSmartChargerClientAlgNode:: Response: %s", set_period_srv_.response.result);
+ //}
+ //else
+ //{
+ //ROS_INFO("DarwinSmartChargerClientAlgNode:: Failed to Call Server on topic set_period ");
+ //}
+
+
+
+ // [fill action structure and make request to the action server]
+
+ // [publish messages]
+}
+
+/* [subscriber callbacks] */
+void DarwinSmartChargerClientAlgNode::smart_charger_data_callback(const darwin_smart_charger_client::smart_charger_data::ConstPtr& msg)
+{
+ // ROS_INFO("DarwinSmartChargerClientAlgNode::smart_charger_data_callback: New Message Received");
+
+ //use appropiate mutex to shared variables if necessary
+ //this->alg_.lock();
+ //this->smart_charger_data_mutex_enter();
+
+ //std::cout << msg->data << std::endl;
+ ROS_INFO_STREAM("Avgerage time to empty: " << msg->avg_time_empty);
+ ROS_INFO_STREAM("Avgerage time to full: " << msg->avg_time_full);
+ ROS_INFO_STREAM("Battery status: " << msg->batt_status);
+ //unlock previously blocked shared variables
+ //this->alg_.unlock();
+ //this->smart_charger_data_mutex_exit();
+}
+
+void DarwinSmartChargerClientAlgNode::smart_charger_data_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->smart_charger_data_mutex_);
+}
+
+void DarwinSmartChargerClientAlgNode::smart_charger_data_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->smart_charger_data_mutex_);
+}
+
+
+/* [service callbacks] */
+
+/* [action callbacks] */
+
+/* [action requests] */
+
+void DarwinSmartChargerClientAlgNode::node_config_update(Config &config, uint32_t level)
+{
+ this->alg_.lock();
+
+//smart_charger_set_config_client
+ //Enable smart charger module
+ if(config.enable)
+ {
+ smart_charger_set_config_srv_.request.enable=true;
+ smart_charger_set_config_srv_.request.disable=false;
+ smart_charger_set_config_srv_.request.set_period=false;
+ smart_charger_set_config_srv_.request.period_value=0.0;
+ smart_charger_set_config_srv_.request.set_limit_current=false;
+ smart_charger_set_config_srv_.request.current_value=0.0;
+
+ ROS_INFO("DarwinSmartChargerClientAlgNode:: Sending New Request! ENABLE");
+ if(smart_charger_set_config_client_.call(smart_charger_set_config_srv_))
+ {
+ //if return ok
+ if(this->smart_charger_set_config_srv_.response.success_en)
+ ROS_INFO("DarwinSmartChargerClientAlgNode:: Smart charger enabled");
+ else
+ ROS_INFO("DarwinSmartChargerClientAlgNode:: Failed to enable smart charger module ");
+ }
+ config.enable=false;
+ }
+//Disable smart charger module
+ if(config.disable && !config.enable)
+ {
+ smart_charger_set_config_srv_.request.disable=true;
+ smart_charger_set_config_srv_.request.enable=false;
+ smart_charger_set_config_srv_.request.set_period=false;
+ smart_charger_set_config_srv_.request.period_value=0.0;
+ smart_charger_set_config_srv_.request.set_limit_current=false;
+ smart_charger_set_config_srv_.request.current_value=0.0;
+
+ ROS_INFO("DarwinSmartChargerClientAlgNode:: Sending New Request! DISABLE");
+ if(smart_charger_set_config_client_.call(smart_charger_set_config_srv_))
+ {
+ if(this->smart_charger_set_config_srv_.response.success_dis)
+ ROS_INFO("DarwinSmartChargerClientAlgNode:: Smart charger disabled");
+ else
+ ROS_INFO("DarwinSmartChargerClientAlgNode:: Failed to disable smart charger module ");
+ }
+ config.disable=false;
+ }
+
+//Set smart charger period
+ if(config.set_period)
+ {
+ smart_charger_set_config_srv_.request.set_period=config.set_period;
+ smart_charger_set_config_srv_.request.period_value=config.period_value;
+ smart_charger_set_config_srv_.request.set_limit_current=false;
+ smart_charger_set_config_srv_.request.current_value=0.0;
+ smart_charger_set_config_srv_.request.enable=false;
+ smart_charger_set_config_srv_.request.disable=false;
+
+ ROS_INFO("DarwinSmartChargerClientAlgNode:: Sending New Request! SET PERIOD");
+ if(this->smart_charger_set_config_client_.call(this->smart_charger_set_config_srv_))
+ {
+ if(this->smart_charger_set_config_srv_.response.success_per)
+ ROS_INFO("DarwinSmartChargerClientAlgNode:: Period changed successfully");
+ else
+ ROS_INFO("DarwinSmartChargerClientAlgNode:: Failed to change period");
+ }
+ config.set_period=false;
+ }
+
+ //Set smart charger limit current
+ if(config.set_limit_current)
+ {
+ smart_charger_set_config_srv_.request.set_limit_current=config.set_limit_current;
+ smart_charger_set_config_srv_.request.current_value=config.current_value;
+ smart_charger_set_config_srv_.request.enable=false;
+ smart_charger_set_config_srv_.request.disable=false;
+ smart_charger_set_config_srv_.request.set_period=false;
+ smart_charger_set_config_srv_.request.period_value=0.0;
+
+ ROS_INFO("DarwinSmartChargerClientAlgNode:: Sending New Request! SET LIMIT CURRENT");
+ if(this->smart_charger_set_config_client_.call(this->smart_charger_set_config_srv_))
+ {
+ if(this->smart_charger_set_config_srv_.response.success_cur)
+ ROS_INFO("DarwinSmartChargerClientAlgNode:: Limit current changed successfully");
+ else
+ ROS_INFO("DarwinSmartChargerClientAlgNode:: Failed to change limit current");
+ }
+ config.set_limit_current=false;
+ }
+
+//smart_charger_get_period_client
+//Get smart charger period
+ if(config.get_period)
+ {
+ ROS_INFO("DarwinSmartChargerClientAlgNode:: Sending New Request! GET PERIOD");
+ if(smart_charger_get_period_client_.call(smart_charger_get_period_srv_))
+ {
+ if(this->smart_charger_get_period_srv_.response.success)
+ {
+ ROS_INFO_STREAM("DarwinSmartChargerClientAlgNode:: Smart charger period:" << smart_charger_get_period_srv_.response.period_value);
+ //else
+ // ROS_INFO("DarwinSmartChargerClientAlgNode:: Failed to Call Server on topic smart_charger_get_period ");
+ }else{
+ ROS_INFO("DarwinSmartChargerClientAlgNode:: Failed to Call Server on topic smart_charger_get_period ");
+ }
+ }
+ config.get_period=false;
+ }
+
+
+ this->config_=config;
+ this->alg_.unlock();
+}
+
+void DarwinSmartChargerClientAlgNode::addNodeDiagnostics(void)
+{
+}
+
+/* main function */
+int main(int argc,char *argv[])
+{
+ return algorithm_base::main<DarwinSmartChargerClientAlgNode>(argc, argv, "darwin_smart_charger_client_alg_node");
+}
diff --git a/darwin_smart_charger_client/srv/get_period.srv b/darwin_smart_charger_client/srv/get_period.srv
new file mode 100644
index 0000000000000000000000000000000000000000..3e3ad3447f1b89313ecf05afece1546a9d6e5937
--- /dev/null
+++ b/darwin_smart_charger_client/srv/get_period.srv
@@ -0,0 +1,3 @@
+---
+float32 period_value
+bool success
\ No newline at end of file
diff --git a/darwin_smart_charger_client/srv/set_config.srv b/darwin_smart_charger_client/srv/set_config.srv
new file mode 100644
index 0000000000000000000000000000000000000000..1f17f29dc5399994c75aa701146a274d9c69c06f
--- /dev/null
+++ b/darwin_smart_charger_client/srv/set_config.srv
@@ -0,0 +1,11 @@
+bool enable
+bool disable
+bool set_period
+float32 period_value
+bool set_limit_current
+float32 current_value
+---
+bool success_en
+bool success_dis
+bool success_per
+bool success_cur
diff --git a/darwin_smart_charger_client/srv/set_limit_current.srv b/darwin_smart_charger_client/srv/set_limit_current.srv
new file mode 100644
index 0000000000000000000000000000000000000000..92bc1b010ccc926f1637cbb2b5689d9a431d6489
--- /dev/null
+++ b/darwin_smart_charger_client/srv/set_limit_current.srv
@@ -0,0 +1,4 @@
+bool set_limit_current
+float32 limit_current
+---
+bool success
\ No newline at end of file
diff --git a/darwin_smart_charger_client/srv/set_period.srv b/darwin_smart_charger_client/srv/set_period.srv
new file mode 100644
index 0000000000000000000000000000000000000000..b803939a723d77387acdb7580df01bea582131cb
--- /dev/null
+++ b/darwin_smart_charger_client/srv/set_period.srv
@@ -0,0 +1,3 @@
+float32 period
+---
+bool ret
\ No newline at end of file
diff --git a/darwin_smart_charger_client/srv/smart_charger_enable.srv b/darwin_smart_charger_client/srv/smart_charger_enable.srv
new file mode 100644
index 0000000000000000000000000000000000000000..24a133ad7f057e80544e328c82546f2749dd790a
--- /dev/null
+++ b/darwin_smart_charger_client/srv/smart_charger_enable.srv
@@ -0,0 +1,3 @@
+bool enable
+---
+bool success
\ No newline at end of file
diff --git a/sm_qr_search/CMakeLists.txt b/sm_qr_search/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..216f38856af7bf3273f272af82c65cf0521f76df
--- /dev/null
+++ b/sm_qr_search/CMakeLists.txt
@@ -0,0 +1,107 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(sm_qr_search)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED)
+# ********************************************************************
+# Add catkin additional components here
+# ********************************************************************
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm nav_msgs geometry_msgs trajectory_msgs control_msgs humanoid_common_msgs actionlib sensor_msgs iri_ros_tools tf)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+# ********************************************************************
+# Add system and labrobotica dependencies here
+# ********************************************************************
+# find_package(<dependency> REQUIRED)
+
+# ********************************************************************
+# Add topic, service and action definition here
+# ********************************************************************
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs # Or other packages containing msgs
+# )
+
+# ********************************************************************
+# Add the dynamic reconfigure file
+# ********************************************************************
+generate_dynamic_reconfigure_options(cfg/SmQrSearch.cfg)
+
+# ********************************************************************
+# Add run time dependencies here
+# ********************************************************************
+catkin_package(
+ INCLUDE_DIRS include
+# LIBRARIES
+# ********************************************************************
+# Add ROS and IRI ROS run time dependencies
+# ********************************************************************
+ CATKIN_DEPENDS iri_base_algorithm nav_msgs geometry_msgs trajectory_msgs control_msgs humanoid_common_msgs actionlib sensor_msgs iri_ros_tools tf
+# ********************************************************************
+# Add system and labrobotica run time dependencies here
+# ********************************************************************
+# DEPENDS
+)
+
+###########
+## Build ##
+###########
+
+# ********************************************************************
+# Add the include directories
+# ********************************************************************
+include_directories(include)
+include_directories(${catkin_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
+# include_directories(${<dependency>_INCLUDE_DIR})
+
+## Declare a cpp library
+# add_library(${PROJECT_NAME} <list of source files>)
+
+## Declare a cpp executable
+add_executable(${PROJECT_NAME} src/sm_qr_search_alg.cpp src/sm_qr_search_alg_node.cpp)
+
+# ********************************************************************
+# Add the libraries
+# ********************************************************************
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+# target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
+
+# ********************************************************************
+# Add message headers dependencies
+# ********************************************************************
+# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} nav_msgs_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} geometry_msgs_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} trajectory_msgs_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} control_msgs_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} humanoid_common_msgs_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} sensor_msgs_generate_messages_cpp)
+# ********************************************************************
+# Add dynamic reconfigure dependencies
+# ********************************************************************
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
diff --git a/sm_qr_search/cfg/SmQrSearch.cfg b/sm_qr_search/cfg/SmQrSearch.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..2f42ab6246cf10f62a97fab7ca5b6f382f21fb5b
--- /dev/null
+++ b/sm_qr_search/cfg/SmQrSearch.cfg
@@ -0,0 +1,85 @@
+#! /usr/bin/env python
+#* All rights reserved.
+#*
+#* Redistribution and use in source and binary forms, with or without
+#* modification, are permitted provided that the following conditions
+#* are met:
+#*
+#* * Redistributions of source code must retain the above copyright
+#* notice, this list of conditions and the following disclaimer.
+#* * Redistributions in binary form must reproduce the above
+#* copyright notice, this list of conditions and the following
+#* disclaimer in the documentation and/or other materials provided
+#* with the distribution.
+#* * Neither the name of the Willow Garage nor the names of its
+#* contributors may be used to endorse or promote products derived
+#* from this software without specific prior written permission.
+#*
+#* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#* POSSIBILITY OF SUCH DAMAGE.
+#***********************************************************
+
+# Author:
+
+PACKAGE='sm_qr_search'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# Name Type Reconfiguration level Description Default Min Max
+#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
+gen.add("start", bool_t, 0, "Start state machine", False)
+gen.add("stop", bool_t, 0, "Stop state machine", False)
+#Joint trajectory
+gen.add("load", bool_t, 0, "Load search parameters", False)
+gen.add("pan_angle_init", double_t, 0, "Initial search pan angle", -1.5, -1.5, 0.0)
+gen.add("pan_angle_end", double_t, 0, "End search pan angle", 1.5, 0.0, 1.5)
+gen.add("tilt_angle_search", double_t, 0, "Search tilt angle", 0.8, 0.0, 1.0)
+gen.add("speed", double_t, 0, "Servo speed (rad/s)", 1, 0, 6.2832)
+gen.add("acceleration", double_t, 0, "Servo acceleration (rad/s/s)", 0.3, 0, 6.2832)
+#Walk
+gen.add("Y_SWAP_AMPLITUDE", double_t, 0, "Y swap amplitude (m)", 0.025, 0, 0.050)
+gen.add("Z_SWAP_AMPLITUDE", double_t, 0, "X swap amplitude (m)", 0.005, 0, 0.050)
+gen.add("ARM_SWING_GAIN", double_t, 0, "Arm swing gain", 1.5, 0, 10.0)
+gen.add("PELVIS_OFFSET", double_t, 0, "Pelvis offset (rad)", 0.052, 0, 0.52)
+gen.add("HIP_PITCH_OFFSET", double_t, 0, "Hip pitch offset (rad)", 0.174, 0, 0.52)
+gen.add("X_OFFSET", double_t, 0, "X offset (m)", -0.025, -0.050, 0.050)
+gen.add("Y_OFFSET", double_t, 0, "Y offset (m)", 0.0075, -0.050, 0.050)
+gen.add("Z_OFFSET", double_t, 0, "Z offset (m)", 0.035, -0.050, 0.050)
+gen.add("A_OFFSET", double_t, 0, "A offset (rad)", 0, -0.52, 0.52)
+gen.add("P_OFFSET", double_t, 0, "P offset (rad)", 0, -0.52, 0.52)
+gen.add("R_OFFSET", double_t, 0, "R offset (rad)", 0, -0.52, 0.52)
+gen.add("PERIOD_TIME", double_t, 0, "Period (s)", 0.6, 0, 1.0)
+gen.add("DSP_RATIO", double_t, 0, "Double support ratio", 0.1, 0, 1.0)
+gen.add("STEP_FB_RATIO", double_t, 0, "Step forward/backward ratio", 0.3, 0, 1.0)
+gen.add("FOOT_HEIGHT", double_t, 0, "Foot height (m)", 0.04, 0, 0.050)
+gen.add("MAX_VEL", double_t, 0, "Maximum linear velocity (m/s)", 0.01, 0, 1.0)
+gen.add("MAX_ROT_VEL", double_t, 0, "Maximum rotational velocity (rad/s)", 0.01, 0, 1.0)
+#Head tracking
+gen.add("pan_p", double_t, 0, "Desired pan kp", 0.1, 0.0, 0.28)
+gen.add("pan_i", double_t, 0, "Desired pan ki", 0.0, 0.0, 0.28)
+gen.add("pan_d", double_t, 0, "Desired pan kd", 0.0, 0.0, 0.28)
+gen.add("update_pan_pid", bool_t, 0, "Update pan pid", False)
+gen.add("pan_i_clamp", double_t, 0, "Desired pan integral limit", 0.0, 0.0, 0.28)
+gen.add("max_pan", double_t, 0, "Maximum pan angle", 3.14159, -3.14159, 3.14159)
+gen.add("min_pan", double_t, 0, "Minimum pan angle", -3.14159, -3.14159, 3.14159)
+gen.add("tilt_p", double_t, 0, "Desired tilt kp", 0.1, 0.0, 0.28)
+gen.add("tilt_i", double_t, 0, "Desired tilt ki", 0.0, 0.0, 0.28)
+gen.add("tilt_d", double_t, 0, "Desired tilt kd", 0.0, 0.0, 0.28)
+gen.add("update_tilt_pid", bool_t, 0, "Update tilt pid", False)
+gen.add("tilt_i_clamp", double_t, 0, "Desired tilt integral limit", 0.0, 0.0, 0.28)
+gen.add("max_tilt", double_t, 0, "Maximum tilt angle", 3.14159, -3.14159, 3.14159)
+gen.add("min_tilt", double_t, 0, "Minimum tilt angle", -3.14159, -3.14159, 3.14159)
+
+exit(gen.generate(PACKAGE, "SmQrSearchAlgorithm", "SmQrSearch"))
diff --git a/sm_qr_search/include/sm_qr_search_alg.h b/sm_qr_search/include/sm_qr_search_alg.h
new file mode 100644
index 0000000000000000000000000000000000000000..06d0e5fa19c18ec9de4312a77a905702eca380ca
--- /dev/null
+++ b/sm_qr_search/include/sm_qr_search_alg.h
@@ -0,0 +1,131 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+// IMPORTANT NOTE: This code has been generated through a script from the
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _sm_qr_search_alg_h_
+#define _sm_qr_search_alg_h_
+
+#include <sm_qr_search/SmQrSearchConfig.h>
+
+//include sm_qr_search_alg main library
+
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ *
+ */
+class SmQrSearchAlgorithm
+{
+ protected:
+ /**
+ * \brief define config type
+ *
+ * Define a Config type with the SmQrSearchConfig. All driver implementations
+ * will then use the same variable type Config.
+ */
+ pthread_mutex_t access_;
+
+ // private attributes and methods
+
+ public:
+ /**
+ * \brief define config type
+ *
+ * Define a Config type with the SmQrSearchConfig. All driver implementations
+ * will then use the same variable type Config.
+ */
+ typedef sm_qr_search::SmQrSearchConfig Config;
+
+ /**
+ * \brief config variable
+ *
+ * This variable has all the driver parameters defined in the cfg config file.
+ * Is updated everytime function config_update() is called.
+ */
+ Config config_;
+
+ /**
+ * \brief constructor
+ *
+ * In this constructor parameters related to the specific driver can be
+ * initalized. Those parameters can be also set in the openDriver() function.
+ * Attributes from the main node driver class IriBaseDriver such as loop_rate,
+ * may be also overload here.
+ */
+ SmQrSearchAlgorithm(void);
+
+ /**
+ * \brief Lock Algorithm
+ *
+ * Locks access to the Algorithm class
+ */
+ void lock(void) { pthread_mutex_lock(&this->access_); };
+
+ /**
+ * \brief Unlock Algorithm
+ *
+ * Unlocks access to the Algorithm class
+ */
+ void unlock(void) { pthread_mutex_unlock(&this->access_); };
+
+ /**
+ * \brief Tries Access to Algorithm
+ *
+ * Tries access to Algorithm
+ *
+ * \return true if the lock was adquired, false otherwise
+ */
+ bool try_enter(void)
+ {
+ if(pthread_mutex_trylock(&this->access_)==0)
+ return true;
+ else
+ return false;
+ };
+
+ /**
+ * \brief config update
+ *
+ * In this function the driver parameters must be updated with the input
+ * config variable. Then the new configuration state will be stored in the
+ * Config attribute.
+ *
+ * \param new_cfg the new driver configuration state
+ *
+ * \param level level in which the update is taken place
+ */
+ void config_update(Config& config, uint32_t level=0);
+
+ // here define all sm_qr_search_alg interface methods to retrieve and set
+ // the driver parameters
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor is called when the object is about to be destroyed.
+ *
+ */
+ ~SmQrSearchAlgorithm(void);
+};
+
+#endif
diff --git a/sm_qr_search/include/sm_qr_search_alg_node.h b/sm_qr_search/include/sm_qr_search_alg_node.h
new file mode 100644
index 0000000000000000000000000000000000000000..33b9fb642ef0d199c9d30644248c7ad95fb94dcf
--- /dev/null
+++ b/sm_qr_search/include/sm_qr_search_alg_node.h
@@ -0,0 +1,220 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+// IMPORTANT NOTE: This code has been generated through a script from the
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _sm_qr_search_alg_node_h_
+#define _sm_qr_search_alg_node_h_
+
+#include <iri_ros_tools/watchdog.h>
+#include <tf/tf.h>
+
+#include <iri_base_algorithm/iri_base_algorithm.h>
+#include "sm_qr_search_alg.h"
+
+// [publisher subscriber headers]
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/Twist.h>
+#include <trajectory_msgs/JointTrajectoryPoint.h>
+#include <humanoid_common_msgs/tag_pose_array.h>
+#include <sensor_msgs/JointState.h>
+
+// [service client headers]
+#include <humanoid_common_msgs/set_walk_params.h>
+#include <humanoid_common_msgs/set_servo_modules.h>
+
+// [action server client headers]
+#include <control_msgs/JointTrajectoryAction.h>
+#include <actionlib/client/simple_action_client.h>
+#include <actionlib/client/terminal_state.h>
+#include <humanoid_common_msgs/humanoid_follow_targetAction.h>
+
+//states
+typedef enum {IDLE, POS_INIT, SEARCH, TRACK_QR, ROTATE, WAIT_ROTATE} States;
+
+/**
+ * \brief IRI ROS Specific Algorithm Class
+ *
+ */
+class SmQrSearchAlgNode : public algorithm_base::IriBaseAlgorithm<SmQrSearchAlgorithm>
+{
+ private:
+ // [publisher attributes]
+ ros::Publisher cmd_vel_publisher_;
+ geometry_msgs::Twist cmd_vel_Twist_msg_;
+
+ ros::Publisher head_target_publisher_;
+ trajectory_msgs::JointTrajectoryPoint head_target_JointTrajectoryPoint_msg_;
+
+
+ // [subscriber attributes]
+ ros::Subscriber odom_subscriber_;
+ void odom_callback(const nav_msgs::Odometry::ConstPtr& msg);
+ pthread_mutex_t odom_mutex_;
+ void odom_mutex_enter(void);
+ void odom_mutex_exit(void);
+
+ ros::Subscriber qr_pose_subscriber_;
+ void qr_pose_callback(const humanoid_common_msgs::tag_pose_array::ConstPtr& msg);
+ pthread_mutex_t qr_pose_mutex_;
+ void qr_pose_mutex_enter(void);
+ void qr_pose_mutex_exit(void);
+
+ ros::Subscriber joint_states_subscriber_;
+ void joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg);
+ pthread_mutex_t joint_states_mutex_;
+ void joint_states_mutex_enter(void);
+ void joint_states_mutex_exit(void);
+
+
+ // [service attributes]
+
+ // [client attributes]
+ ros::ServiceClient set_walk_params_client_;
+ humanoid_common_msgs::set_walk_params set_walk_params_srv_;
+
+ ros::ServiceClient set_servo_modules_client_;
+ humanoid_common_msgs::set_servo_modules set_servo_modules_srv_;
+
+
+ // [action server attributes]
+
+ // [action client attributes]
+ actionlib::SimpleActionClient<control_msgs::JointTrajectoryAction> joint_trajectory_client_;
+ control_msgs::JointTrajectoryGoal joint_trajectory_goal_;
+ bool joint_trajectoryMakeActionRequest();
+ void joint_trajectoryDone(const actionlib::SimpleClientGoalState& state, const control_msgs::JointTrajectoryResultConstPtr& result);
+ void joint_trajectoryActive();
+ void joint_trajectoryFeedback(const control_msgs::JointTrajectoryFeedbackConstPtr& feedback);
+
+ actionlib::SimpleActionClient<humanoid_common_msgs::humanoid_follow_targetAction> head_follow_target_client_;
+ humanoid_common_msgs::humanoid_follow_targetGoal head_follow_target_goal_;
+ bool head_follow_targetMakeActionRequest();
+ void head_follow_targetDone(const actionlib::SimpleClientGoalState& state, const humanoid_common_msgs::humanoid_follow_targetResultConstPtr& result);
+ void head_follow_targetActive();
+ void head_follow_targetFeedback(const humanoid_common_msgs::humanoid_follow_targetFeedbackConstPtr& feedback);
+
+ States state;
+ bool new_goal;
+ bool end;
+ bool executing_trajectory;
+ double current_pan_angle;
+ double current_tilt_angle;
+ double next_pan_angle;
+ double next_tilt_angle;
+ double pan_angle_init;
+ double pan_angle_end;
+ double tilt_angle_search;
+ //qr
+ double pan_angle;
+ double tilt_angle;
+ bool tracking;
+ bool qr_detected;
+ unsigned int counter;
+ //odometry
+ double odom_x;
+ double odom_y;
+ double odom_rot;
+
+ CROSWatchdog watchdog_qr_lost;
+ //CROSWatchdog watchdog_walk;
+ //CROSWatchdog feedback_watchdog;
+
+ /**
+ * \brief config variable
+ *
+ * This variable has all the driver parameters defined in the cfg config file.
+ * Is updated everytime function config_update() is called.
+ */
+ Config config_;
+ public:
+ /**
+ * \brief Constructor
+ *
+ * This constructor initializes specific class attributes and all ROS
+ * communications variables to enable message exchange.
+ */
+ SmQrSearchAlgNode(void);
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor frees all necessary dynamic memory allocated within this
+ * this class.
+ */
+ ~SmQrSearchAlgNode(void);
+
+ protected:
+ /**
+ * \brief main node thread
+ *
+ * This is the main thread node function. Code written here will be executed
+ * in every node loop while the algorithm is on running state. Loop frequency
+ * can be tuned by modifying loop_rate attribute.
+ *
+ * Here data related to the process loop or to ROS topics (mainly data structs
+ * related to the MSG and SRV files) must be updated. ROS publisher objects
+ * must publish their data in this process. ROS client servers may also
+ * request data to the corresponding server topics.
+ */
+ void mainNodeThread(void);
+
+ /**
+ * \brief dynamic reconfigure server callback
+ *
+ * This method is called whenever a new configuration is received through
+ * the dynamic reconfigure. The derivated generic algorithm class must
+ * implement it.
+ *
+ * \param config an object with new configuration from all algorithm
+ * parameters defined in the config file.
+ * \param level integer referring the level in which the configuration
+ * has been changed.
+ */
+ void node_config_update(Config &config, uint32_t level);
+
+ /**
+ * \brief node add diagnostics
+ *
+ * In this abstract function additional ROS diagnostics applied to the
+ * specific algorithms may be added.
+ */
+ void addNodeDiagnostics(void);
+
+ // [diagnostic functions]
+
+ // [test functions]
+
+ bool set_servo_module_head(std::string module);
+ bool set_servo_module(std::string module);
+ //Walk
+ bool set_walk_params(void);
+ void send_cmd_vel(double x_amplitude, double y_amplitude, double a_amplitude);
+ //Joints module
+ void joint_trajectory_set_goal(double pan_position, double tilt_position);
+ bool joint_trajectory_start_action(void);
+ //Head tracking
+ void set_head_tracking_goal(double pan_target, double tilt_target);
+ void send_head_target(double pan, double tilt);
+};
+
+#endif
diff --git a/sm_qr_search/launch/search_sim.launch b/sm_qr_search/launch/search_sim.launch
new file mode 100644
index 0000000000000000000000000000000000000000..0623581730748f7fd123ea7a3dad8d2be290117d
--- /dev/null
+++ b/sm_qr_search/launch/search_sim.launch
@@ -0,0 +1,69 @@
+<launch>
+
+ <arg name="robot" default="darwin" />
+ <arg name="environment" default="vision_env" />
+
+
+ <include file="$(find darwin_description)/launch/darwin_sim.launch">
+ <arg name="robot" value="$(arg robot)" />
+ </include>
+
+ <include file="$(find bioloid_description)/launch/vision_env.launch">
+ <arg name="environment" value="$(arg environment)" />
+ </include>
+
+ <!-- launch the action client node -->
+ <node name="sm_qr_search"
+ pkg="sm_qr_search"
+ type="sm_qr_search"
+ output="screen"
+ ns="/darwin">
+ <remap from="/darwin/sm_qr_search/cmd_vel"
+ to="/darwin/robot/cmd_vel"/>
+ <remap from="/darwin/sm_qr_search/set_walk_params"
+ to="/darwin/robot/set_walk_params"/>
+ <remap from="/darwin/sm_qr_search/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/head_follow_target"
+ to="/darwin/robot/head_follow_target"/>
+ <remap from="/darwin/sm_qr_search/head_target"
+ to="/darwin/robot/head_target"/>
+ <remap from="/darwin/sm_qr_search/joint_states"
+ to="/darwin/joint_states"/>
+ <remap from="/darwin/sm_qr_search/qr_pose"
+ to="/qr_detector/qr_pose"/>
+ <remap from="/darwin/joint_trajectory"
+ to="/darwin/robot/joint_trajectory"/>
+ </node>
+
+ <!-- <include file="$(find darwin_apps)/launch/xs_camera_qr.launch"/>-->
+ <include file="$(find qr_detector)/launch/tracker_darwin.launch"/>
+
+ <node pkg="image_view"
+ name="image_view"
+ type="image_view"
+ args="image:=/darwin/usb_cam/image_raw">
+ </node>
+
+ <!-- launch dynamic reconfigure -->
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+ output="screen"/>
+
+ <!-- load the controllers -->
+ <node name="darwin_odometry"
+ pkg="darwin_odometry"
+ type="darwin_odometry"
+ output="screen"
+ ns="/darwin">
+ <param name="left_leg_chain_file" value="$(find darwin_odometry)/config/left_leg.xml"/>
+ <param name="right_leg_chain_file" value="$(find darwin_odometry)/config/right_leg.xml"/>
+ <param name="tf_prefix" value="/darwin"/>
+ <param name="publish_tf" value="True"/>
+ <remap from="/darwin/darwin_odometry/joint_states"
+ to="/darwin/joint_states"/>
+ <remap from="/darwin/darwin_odometry/odom"
+ to="/darwin/sm_qr_search/odom"/>
+ </node>
+
+</launch>
+
diff --git a/sm_qr_search/package.xml b/sm_qr_search/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..3f573e4fe30f930c160a701baca0ddaca1561582
--- /dev/null
+++ b/sm_qr_search/package.xml
@@ -0,0 +1,70 @@
+<?xml version="1.0"?>
+<package>
+ <name>sm_qr_search</name>
+ <version>0.0.0</version>
+ <description>The sm_qr_search package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="igarcia@todo.todo">igarcia</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>LGPL</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/sm_qr_search</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>iri_base_algorithm</build_depend>
+ <build_depend>nav_msgs</build_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_depend>trajectory_msgs</build_depend>
+ <build_depend>control_msgs</build_depend>
+ <build_depend>humanoid_common_msgs</build_depend>
+ <build_depend>actionlib</build_depend>
+ <build_depend>sensor_msgs</build_depend>
+ <build_depend>iri_ros_tools</build_depend>
+ <build_depend>tf</build_depend>
+ <run_depend>iri_base_algorithm</run_depend>
+ <run_depend>nav_msgs</run_depend>
+ <run_depend>geometry_msgs</run_depend>
+ <run_depend>trajectory_msgs</run_depend>
+ <run_depend>control_msgs</run_depend>
+ <run_depend>humanoid_common_msgs</run_depend>
+ <run_depend>actionlib</run_depend>
+ <run_depend>sensor_msgs</run_depend>
+ <run_depend>iri_ros_tools</run_depend>
+ <run_depend>tf</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/sm_qr_search/sm_qr_search.launch b/sm_qr_search/sm_qr_search.launch
new file mode 100644
index 0000000000000000000000000000000000000000..e62af4d123b4a0062bf44b9d532e906923d74341
--- /dev/null
+++ b/sm_qr_search/sm_qr_search.launch
@@ -0,0 +1,88 @@
+<launch>
+
+ <arg name="robot" default="darwin" />
+
+ <include file="$(find darwin_description)/launch/darwin_base.launch">
+ <arg name="robot" value="$(arg robot)" />
+ </include>
+
+ <!-- launch the QR head tracking node -->
+ <node name="sm_qr_search"
+ pkg="sm_qr_search"
+ type="sm_qr_search"
+ output="screen"
+ ns="/darwin">
+<!-- <param name="load" value="false"/>
+ <param name="pan_angle_init" value="-1.5"/>
+ <param name="pan_angle_end" value="1.5"/>
+ <param name="tilt_angle_search" value="0.8"/>
+ <param name="speed" value="1"/>
+ <param name="acceleration" value="0.3"/>
+
+ <param name="Y_SWAP_AMPLITUDE" value="0.020"/>
+ <param name="Z_SWAP_AMPLITUDE" value="0.005"/>
+ <param name="ARM_SWING_GAIN" value="1.5"/>
+ <param name="PELVIS_OFFSET" value="0.05"/>
+ <param name="HIP_PITCH_OFFSET" value="0.23"/>
+ <param name="X_OFFSET" value="-0.01"/>
+ <param name="Y_OFFSET" value="0.005"/>
+ <param name="Z_OFFSET" value="0.02"/>
+ <param name="A_OFFSET" value="0.0"/>
+ <param name="P_OFFSET" value="0.0"/>
+ <param name="R_OFFSET" value="0.0"/>
+ <param name="PERIOD_TIME" value="0.6"/>
+ <param name="DSP_RATIO" value="0.1"/>
+ <param name="STEP_FB_RATIO" value="0.28"/>
+ <param name="FOOT_HEIGHT" value="0.04"/>
+ <param name="MAX_VEL" value="0.01"/>
+ <param name="MAX_ROT_VEL" value="0.01"/> -->
+
+ <remap from="/darwin/sm_qr_search/cmd_vel"
+ to="/darwin/robot/cmd_vel"/>
+ <remap from="/darwin/sm_qr_search/set_walk_params"
+ to="/darwin/robot/set_walk_params"/>
+ <remap from="/darwin/sm_qr_search/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/head_follow_target"
+ to="/darwin/robot/head_follow_target"/>
+ <remap from="/darwin/sm_qr_search/head_target"
+ to="/darwin/robot/head_target"/>
+ <remap from="/darwin/sm_qr_search/joint_states"
+ to="/darwin/joint_states"/>
+ <remap from="/darwin/sm_qr_search/qr_pose"
+ to="/qr_detector/qr_pose"/>
+ <remap from="/darwin/joint_trajectory"
+ to="/darwin/robot/joint_trajectory"/>
+ </node>
+
+<!-- <include file="$(find darwin_apps)/launch/xs_camera_qr.launch"/>-->
+ <include file="$(find qr_detector)/launch/tracker_darwin.launch"/>
+
+ <node pkg="image_view"
+ name="image_view"
+ type="image_view"
+ args="image:=/darwin/usb_cam/image_raw">
+ </node>
+
+ <!-- launch dynamic reconfigure -->
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+ output="screen"/>
+
+
+ <!-- load the controllers -->
+ <node name="darwin_odometry"
+ pkg="darwin_odometry"
+ type="darwin_odometry"
+ output="screen"
+ ns="/darwin">
+ <param name="left_leg_chain_file" value="$(find darwin_odometry)/config/left_leg.xml"/>
+ <param name="right_leg_chain_file" value="$(find darwin_odometry)/config/right_leg.xml"/>
+ <param name="tf_prefix" value="/darwin"/>
+ <param name="publish_tf" value="True"/>
+ <remap from="/darwin/darwin_odometry/joint_states"
+ to="/darwin/joint_states"/>
+ <remap from="/darwin/darwin_odometry/odom"
+ to="/darwin/sm_qr_search/odom"/>
+ </node>
+
+</launch>
\ No newline at end of file
diff --git a/sm_qr_search/src/sm_qr_search_alg.cpp b/sm_qr_search/src/sm_qr_search_alg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4903c127c3cc00412f0555ccb1147a7c2ac07d4f
--- /dev/null
+++ b/sm_qr_search/src/sm_qr_search_alg.cpp
@@ -0,0 +1,23 @@
+#include "sm_qr_search_alg.h"
+
+SmQrSearchAlgorithm::SmQrSearchAlgorithm(void)
+{
+ pthread_mutex_init(&this->access_,NULL);
+}
+
+SmQrSearchAlgorithm::~SmQrSearchAlgorithm(void)
+{
+ pthread_mutex_destroy(&this->access_);
+}
+
+void SmQrSearchAlgorithm::config_update(Config& config, uint32_t level)
+{
+ this->lock();
+
+ // save the current configuration
+ this->config_=config;
+
+ this->unlock();
+}
+
+// SmQrSearchAlgorithm Public API
diff --git a/sm_qr_search/src/sm_qr_search_alg_node.cpp b/sm_qr_search/src/sm_qr_search_alg_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6ea959bbec09055bf4ad103fcb3eb8802ab1f9f4
--- /dev/null
+++ b/sm_qr_search/src/sm_qr_search_alg_node.cpp
@@ -0,0 +1,924 @@
+
+/* SEARCH QR ALGORITHM
+ *
+ * Moves head 180º. Since -pi/2 to pi/2
+ * Rotates body 180º
+ *
+ PROBLEMAS: wathdog timer crea expcepcion y se queda bloqueado. contador
+ */
+
+
+#include "sm_qr_search_alg_node.h"
+
+#define NUM_SERVOS 32
+// joint names
+const std::string servo_names[NUM_SERVOS]={std::string("Servo0"),
+ std::string("j_shoulder_pitch_r"),
+ std::string("j_shoulder_pitch_l"),
+ std::string("j_shoulder_roll_r"),
+ std::string("j_shoulder_roll_l"),
+ std::string("j_elbow_r"),
+ std::string("j_elbow_l"),
+ std::string("j_hip_yaw_r"),
+ std::string("j_hip_yaw_l"),
+ std::string("j_hip_roll_r"),
+ std::string("j_hip_roll_l"),
+ std::string("j_hip_pitch_r"),
+ std::string("j_hip_pitch_l"),
+ std::string("j_knee_r"),
+ std::string("j_knee_l"),
+ std::string("j_ankle_pitch_r"),
+ std::string("j_ankle_pitch_l"),
+ std::string("j_ankle_roll_r"),
+ std::string("j_ankle_roll_l"),
+ std::string("j_pan"),
+ std::string("j_tilt"),
+ std::string("Servo21"),
+ std::string("Servo22"),
+ std::string("Servo23"),
+ std::string("Servo24"),
+ std::string("Servo25"),
+ std::string("Servo26"),
+ std::string("Servo27"),
+ std::string("Servo28"),
+ std::string("Servo29"),
+ std::string("Servo30"),
+ std::string("Servo31")};
+
+SmQrSearchAlgNode::SmQrSearchAlgNode(void) :
+ algorithm_base::IriBaseAlgorithm<SmQrSearchAlgorithm>(),
+ joint_trajectory_client_("joint_trajectory", true),
+ head_follow_target_client_("head_follow_target", true)
+{
+ //init class attributes if necessary
+ //this->loop_rate_ = 2;//in [Hz]
+
+ // [init publishers]
+ this->cmd_vel_publisher_ = this->public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+ this->head_target_publisher_ = this->public_node_handle_.advertise<trajectory_msgs::JointTrajectoryPoint>("head_target", 1);
+
+ // [init subscribers]
+ this->odom_subscriber_ = this->public_node_handle_.subscribe("odom", 1, &SmQrSearchAlgNode::odom_callback, this);
+ pthread_mutex_init(&this->odom_mutex_,NULL);
+
+ this->qr_pose_subscriber_ = this->public_node_handle_.subscribe("qr_pose", 1, &SmQrSearchAlgNode::qr_pose_callback, this);
+ pthread_mutex_init(&this->qr_pose_mutex_,NULL);
+
+ this->joint_states_subscriber_ = this->public_node_handle_.subscribe("joint_states", 1, &SmQrSearchAlgNode::joint_states_callback, this);
+ pthread_mutex_init(&this->joint_states_mutex_,NULL);
+
+
+ // [init services]
+
+ // [init clients]
+ set_walk_params_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_walk_params>("set_walk_params");
+
+ set_servo_modules_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_servo_modules>("set_servo_modules");
+
+
+ // [init action servers]
+
+ // [init action clients]
+
+ this->state=IDLE;
+ this->new_goal=false;
+ this->end=false;
+ this->executing_trajectory=false;
+ this->next_pan_angle=-1.5;
+ this->next_tilt_angle=0.8;
+
+ this->pan_angle_init=-1.5;
+ this->pan_angle_end=1.5;
+ this->tilt_angle_search=0.8;
+
+ this->current_pan_angle=0.0;
+ this->current_tilt_angle=0.0;
+ //qr
+ this->qr_detected=false;
+ this->pan_angle=0.0;
+ this->tilt_angle=0.0;
+ this->tracking=false;
+ this->counter=0;
+
+
+ actionlib::SimpleClientGoalState joint_trajectory_state(actionlib::SimpleClientGoalState::PENDING);
+
+
+ //head_follow
+ this->head_follow_target_goal_.pan_range.resize(2);
+ this->head_follow_target_goal_.tilt_range.resize(2);
+
+ //joint_trajectory
+ this->joint_trajectory_goal_.trajectory.points.resize(1);
+ this->joint_trajectory_goal_.trajectory.joint_names.resize(2);
+ this->joint_trajectory_goal_.trajectory.points[0].positions.resize(2);
+ this->joint_trajectory_goal_.trajectory.points[0].velocities.resize(2);
+ this->joint_trajectory_goal_.trajectory.points[0].accelerations.resize(2);
+
+
+ //head_target
+ this->head_target_JointTrajectoryPoint_msg_.positions.resize(2);
+ //this->head_target_JointTrajectoryPoint_msg_.velocities.resize(2);
+
+}
+
+SmQrSearchAlgNode::~SmQrSearchAlgNode(void)
+{
+ // [free dynamic memory]
+ pthread_mutex_destroy(&this->odom_mutex_);
+ pthread_mutex_destroy(&this->qr_pose_mutex_);
+ pthread_mutex_destroy(&this->joint_states_mutex_);
+}
+
+void SmQrSearchAlgNode::mainNodeThread(void)
+{
+/* // [fill msg structures]
+ // Initialize the topic message structure
+ //this->cmd_vel_Twist_msg_.data = my_var;
+
+ // Initialize the topic message structure
+ //this->head_target_JointTrajectoryPoint_msg_.data = my_var;
+
+
+ // [fill srv structure and make request to the server]
+ //set_walk_params_srv_.request.data = my_var;
+ //ROS_INFO("SmQrSearchAlgNode:: Sending New Request!");
+ //if (set_walk_params_client_.call(set_walk_params_srv_))
+ //{
+ //ROS_INFO("SmQrSearchAlgNode:: Response: %s", set_walk_params_srv_.response.result);
+ //}
+ //else
+ //{
+ //ROS_INFO("SmQrSearchAlgNode:: Failed to Call Server on topic set_walk_params ");
+ //}
+
+
+ //set_servo_modules_srv_.request.data = my_var;
+ //ROS_INFO("SmQrSearchAlgNode:: Sending New Request!");
+ //if (set_servo_modules_client_.call(set_servo_modules_srv_))
+ //{
+ //ROS_INFO("SmQrSearchAlgNode:: Response: %s", set_servo_modules_srv_.response.result);
+ //}
+ //else
+ //{
+ //ROS_INFO("SmQrSearchAlgNode:: Failed to Call Server on topic set_servo_modules ");
+ //}
+
+
+
+ // [fill action structure and make request to the action server]
+ // variable to hold the state of the current goal on the server
+ //actionlib::SimpleClientGoalState joint_trajectory_state(actionlib::SimpleClientGoalState::PENDING);
+ // to get the state of the current goal
+ //alg_.unlock();
+ //joint_trajectory_state=joint_trajectory_client_.getState();
+ // Possible state values are: PENDING,ACTIVE,RECALLED,REJECTED,PREEMPTED,ABORTED,SUCCEEDED and LOST
+ //alg_.lock();
+ //if(joint_trajectory_state==actionlib::SimpleClientGoalState::ABORTED)
+ //{
+ // do something
+ //}
+ //else if(joint_trajectory_state==actionlib::SimpleClientGoalState::SUCCEEDED)
+ //{
+ // do something else
+ //}
+ //joint_trajectoryMakeActionRequest();
+
+ // variable to hold the state of the current goal on the server
+ //actionlib::SimpleClientGoalState head_follow_target_state(actionlib::SimpleClientGoalState::PENDING);
+ // to get the state of the current goal
+ //alg_.unlock();
+ //head_follow_target_state=head_follow_target_client_.getState();
+ // Possible state values are: PENDING,ACTIVE,RECALLED,REJECTED,PREEMPTED,ABORTED,SUCCEEDED and LOST
+ //alg_.lock();
+ //if(head_follow_target_state==actionlib::SimpleClientGoalState::ABORTED)
+ //{
+ // do something
+ //}
+ //else if(head_follow_target_state==actionlib::SimpleClientGoalState::SUCCEEDED)
+ //{
+ // do something else
+ //}
+ //head_follow_targetMakeActionRequest();
+
+ // IMPORTANT: Please note that all mutex used in the client callback functions
+ // must be unlocked before calling any of the client class functions from an
+ // other thread (MainNodeThread).
+
+
+ // [publish messages]
+ // Uncomment the following line to publish the topic message
+ //this->cmd_vel_publisher_.publish(this->cmd_vel_Twist_msg_);
+
+ // Uncomment the following line to publish the topic message
+ //this->head_target_publisher_.publish(this->head_target_JointTrajectoryPoint_msg_);
+
+ */
+
+
+ switch(state)
+ {
+ case IDLE:
+ //Poner el robot a la posicion de inicio
+ ROS_INFO("IDLE");
+ if(new_goal)
+ {
+ new_goal=false;
+ //this->next_pan_angle=-1.5;
+ set_servo_module_head("joints"); //set head servos to joints
+ joint_trajectory_set_goal(pan_angle_init, tilt_angle_search);
+ if(joint_trajectory_start_action())
+ {
+ // ROS_INFO("Start Joint trajectory action");
+ state=POS_INIT;
+ }
+ }
+ else
+ state=IDLE;
+ break;
+
+ //Wait joint trajectory to execute
+ case POS_INIT:
+ //ROS_INFO("POS_INIT");
+ if(executing_trajectory){
+ ROS_INFO("Going to initial position");
+ state=POS_INIT;
+ }else{
+ joint_trajectory_set_goal(pan_angle_end, tilt_angle_search);
+ if(joint_trajectory_start_action())
+ {
+ ROS_INFO("Start search");
+ state=SEARCH;
+ }
+ }
+ break;
+
+
+ case SEARCH:
+ ROS_INFO("SEARCH");
+ if(qr_detected){
+ ROS_INFO("QR detected");
+ this->joint_trajectory_client_.cancelGoal();
+ set_servo_module_head("head");
+ this->tracking=true;
+ set_head_tracking_goal(this->current_pan_angle, this->current_tilt_angle);
+ if(head_follow_targetMakeActionRequest())
+ {
+ state=TRACK_QR;
+ }
+ }else{
+ if(!executing_trajectory){
+ ROS_INFO("End of search");
+ state=IDLE;
+ }else{
+ ROS_INFO("current pan angle: %f", this->current_pan_angle);
+ }
+ }
+
+ break;
+
+ /*
+ case TRACK_QR:
+ ROS_INFO("TRACK QR");
+ if(this->watchdog_qr_lost.is_active())
+ //if(!qr_detected)
+ {
+ ROS_INFO("QR lost");
+ this->tracking=false;
+ this->head_follow_target_client_.cancelGoal();
+ set_servo_module_head("joints");
+ joint_trajectory_set_goal(pan_angle_init, tilt_angle_search);
+ if(joint_trajectory_start_action())
+ {
+ // ROS_INFO("Start Joint trajectory action");
+ state=POS_INIT;
+ }
+
+ }
+ else if(end)
+ {
+ this->end=false;
+ this->tracking=false;
+ this->qr_detected=false;
+ this->head_follow_target_client_.cancelGoal();
+ state=IDLE;
+ }
+ else
+ {
+ counter=0;
+ state=TRACK_QR;
+ }
+ break;
+*/
+ case TRACK_QR:
+ ROS_INFO("TRACK QR");
+ // if(this->watchdog_qr_lost.is_active())
+ // if(!qr_detected)
+ //{
+ if(counter==10)
+ {
+ ROS_INFO("QR lost");
+ this->tracking=false;
+ this->head_follow_target_client_.cancelGoal();
+ set_servo_module_head("joints");
+ joint_trajectory_set_goal(pan_angle_init, tilt_angle_search);
+ if(joint_trajectory_start_action())
+ {
+ // ROS_INFO("Start Joint trajectory action");
+ state=POS_INIT;
+ }
+ }
+ //}
+ else if(end)
+ {
+ this->end=false;
+ this->tracking=false;
+ this->qr_detected=false;
+ this->head_follow_target_client_.cancelGoal();
+ state=IDLE;
+ }
+ else
+ {
+ counter=0;
+ state=TRACK_QR;
+ }
+ break;
+
+ /* case ROTATE:
+ ROS_INFO("Set servos to 'walk'");
+ set_servo_module("walk");
+ ROS_INFO("Set Walk params");
+ if(set_walk_params(void))
+ {
+ ROS_INFO("Turning around");
+ send_cmd_vel(0,0,1);
+ //sleep(20);
+ state=WAIT_ROTATE;
+ }
+ break;
+
+ case WAIT_ROTATE:
+ if(watchdog_rotate.is_active())
+ {
+ send_cmd_vel(0,0,0);
+ ROS_INFO("Set head servos to 'joints'");
+ set_servo_module_head("joints");
+ joint_trajectory_set_goal(pan_angle_init, tilt_angle_search);
+ if(joint_trajectory_start_action())
+ {
+ state=POS_INIT;
+ }
+ }
+ else
+ {
+ state=WAIT_ROTATE;
+ break;
+ */
+
+
+
+ /* case ACTION:
+ ROS_INFO("NavModule: nav_action_wait");
+ if(nav_module->use_nav_timeout && nav_module->nav_timeout.timed_out())
+ {
+ ROS_ERROR("NavModule: goal timed out");
+ // cancel the current action
+ this->nav_access.exit();
+ this->head_follow_target_client_.cancelGoal();
+ this->nav_access.enter();
+ // stop the timer
+ nav_module->nav_timeout.stop();
+ // set the error
+ nav_module->state=HANDLE_ERROR;
+ }
+ else if(this->->feedback_watchdog.is_active())
+ {
+ ROS_ERROR("NavModule: goal feedback watchdog, %fs", nav_module->config.move_feedback_watchdog);
+ // cancel the current action
+ nav_module->nav_access.exit();
+ this->move_base_client->cancelGoal();
+ nav_module->nav_access.enter();
+ // set the error
+ nav_module->status=NAV_MODULE_FB_WATCHDOG;
+ this->state=HANDLE_ERROR;
+ }
+ else
+ {
+ nav_module->nav_access.exit();
+ joint_trajectory_state=nav_module->move_base_client->getState();
+ nav_module->nav_access.enter();
+ if(joint_trajectory_state!=actionlib::SimpleClientGoalState::ACTIVE)
+ {
+ if(joint_trajectory_state==actionlib::SimpleClientGoalState::ABORTED)
+ {
+ ROS_ERROR("NavModule: goal aborted");
+ nav_module->status=NAV_MODULE_ABORTED;
+ }
+ else if(joint_trajectory_state==actionlib::SimpleClientGoalState::PREEMPTED)
+ {
+ ROS_WARN("NavModule: goal preempted");
+ nav_module->status=NAV_MODULE_PREEMPTED;
+ }
+ else if(joint_trajectory_state==actionlib::SimpleClientGoalState::SUCCEEDED)
+ {
+ ROS_DEBUG("NavModule: goal successfull");
+ nav_module->status=NAV_MODULE_SUCCESS;
+ }
+
+ nav_module->running=false;
+ nav_module->state=nav_idle;
+ }
+ else
+ {
+ ROS_DEBUG("NavModule: goal active");
+ nav_module->state=nav_action_wait;
+ }
+ }
+ if(nav_module->cancel_pending)
+ {
+ nav_module->cancel_pending=false;
+ nav_module->nav_access.exit();
+ nav_module->move_base_client->cancelGoal();
+ nav_module->nav_access.enter();
+ }
+ break;
+ */
+
+
+
+
+
+}
+
+}
+
+/* [subscriber callbacks] */
+void SmQrSearchAlgNode::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
+{
+ //ROS_INFO("SmQrSearchAlgNode::odom_callback: New Message Received");
+
+ //use appropiate mutex to shared variables if necessary
+ //this->alg_.lock();
+ this->odom_mutex_enter();
+
+ odom_x=msg->pose.pose.position.x;
+ odom_y=msg->pose.pose.position.y;
+ //odom_rot=msg->pose.pose.orientation;
+ odom_rot=tf::getYaw(msg->pose.pose.orientation);
+ ROS_INFO("Orientacion Odometria: %f", odom_rot);
+
+ //unlock previously blocked shared variables
+ //this->alg_.unlock();
+ this->odom_mutex_exit();
+}
+
+void SmQrSearchAlgNode::odom_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->odom_mutex_);
+}
+
+void SmQrSearchAlgNode::odom_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->odom_mutex_);
+}
+
+void SmQrSearchAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_pose_array::ConstPtr& msg)
+{
+ //ROS_INFO("SmQrSearchAlgNode::qr_pose_callback: New Message Received");
+
+ //use appropiate mutex to shared variables if necessary
+ //this->alg_.lock();
+ this->qr_pose_mutex_enter();
+
+ if(msg->tags.size()>0)
+ {
+ //qr_detected=msg->tags.size();
+ qr_detected=true;
+
+ this->pan_angle=this->current_pan_angle+atan2(-msg->tags[0].position.x,msg->tags[0].position.z);
+ this->tilt_angle=this->current_tilt_angle+atan2(-msg->tags[0].position.y,msg->tags[0].position.z);
+ //ROS_INFO("Next target pan angle: %f (%f,%f)",this->pan_angle,this->current_pan_angle,atan2(msg->tags[0].position.x,msg->tags[0].position.z));
+ //ROS_INFO("Next target tilt angle: %f (%f,%f)",this->tilt_angle,this->current_tilt_angle,atan2(msg->tags[0].position.y,msg->tags[0].position.z));
+
+ if(this->tracking)
+ {
+ send_head_target(pan_angle, tilt_angle);
+ }
+
+ // this->watchdog_qr_lost.reset(ros::Duration(5));
+ counter=0;
+ }else{
+ ROS_INFO("QR not detected");
+ qr_detected=false;
+ counter++;
+
+ }
+
+
+
+ //unlock previously blocked shared variables
+ //this->alg_.unlock();
+ this->qr_pose_mutex_exit();
+
+}
+
+void SmQrSearchAlgNode::qr_pose_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->qr_pose_mutex_);
+}
+
+void SmQrSearchAlgNode::qr_pose_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->qr_pose_mutex_);
+}
+
+void SmQrSearchAlgNode::joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg)
+{
+ //ROS_INFO("SmQrSearchAlgNode::joint_states_callback: New Message Received");
+
+ //use appropiate mutex to shared variables if necessary
+ //this->alg_.lock();
+ this->joint_states_mutex_enter();
+
+
+ unsigned int i;
+
+ for (i = 0; i < msg->name.size() ; ++i){
+ if (msg->name[i]=="j_pan"){
+ this->current_pan_angle=msg->position[i];
+ }
+ else if (msg->name[i]=="j_tilt"){
+ this->current_tilt_angle=(msg->position[i]);
+ }
+ }
+
+ //unlock previously blocked shared variables
+ //this->alg_.unlock();
+ this->joint_states_mutex_exit();
+
+}
+
+void SmQrSearchAlgNode::joint_states_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->joint_states_mutex_);
+}
+
+void SmQrSearchAlgNode::joint_states_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->joint_states_mutex_);
+}
+
+
+/* [service callbacks] */
+
+/* [action callbacks] */
+void SmQrSearchAlgNode::joint_trajectoryDone(const actionlib::SimpleClientGoalState& state, const control_msgs::JointTrajectoryResultConstPtr& result)
+{
+ alg_.lock();
+ if( state == actionlib::SimpleClientGoalState::SUCCEEDED )
+ {
+ //ROS_INFO("SmQrSearchAlgNode::joint_trajectoryDone: Goal Achieved!");
+ this->executing_trajectory=false;
+ //next_pan_angle=
+ }
+ else
+ ROS_INFO("SmQrSearchAlgNode::joint_trajectoryDone: %s", state.toString().c_str());
+
+ //copy & work with requested result
+ alg_.unlock();
+}
+
+void SmQrSearchAlgNode::joint_trajectoryActive()
+{
+ alg_.lock();
+ //ROS_INFO("SmQrSearchAlgNode::joint_trajectoryActive: Goal just went active!");
+ alg_.unlock();
+}
+
+void SmQrSearchAlgNode::joint_trajectoryFeedback(const control_msgs::JointTrajectoryFeedbackConstPtr& feedback)
+{
+ alg_.lock();
+ //ROS_INFO("SmQrSearchAlgNode::joint_trajectoryFeedback: Got Feedback!");
+
+ bool feedback_is_ok = true;
+
+ //this->feedback_watchdog.reset(ros::Duration(2));
+
+ //analyze feedback
+ //my_var = feedback->var;
+
+ //if feedback is not what expected, cancel requested goal
+ if( !feedback_is_ok )
+ {
+ joint_trajectory_client_.cancelGoal();
+ //ROS_INFO("SmQrSearchAlgNode::joint_trajectoryFeedback: Cancelling Action!");
+ }
+ alg_.unlock();
+}
+
+void SmQrSearchAlgNode::head_follow_targetDone(const actionlib::SimpleClientGoalState& state, const humanoid_common_msgs::humanoid_follow_targetResultConstPtr& result)
+{
+ alg_.lock();
+ if( state == actionlib::SimpleClientGoalState::SUCCEEDED )
+ ROS_INFO("SmQrSearchAlgNode::head_follow_targetDone: Goal Achieved!");
+ else
+ ROS_INFO("SmQrSearchAlgNode::head_follow_targetDone: %s", state.toString().c_str());
+
+ //copy & work with requested result
+ alg_.unlock();
+}
+
+void SmQrSearchAlgNode::head_follow_targetActive()
+{
+ alg_.lock();
+ //ROS_INFO("SmQrSearchAlgNode::head_follow_targetActive: Goal just went active!");
+ alg_.unlock();
+}
+
+void SmQrSearchAlgNode::head_follow_targetFeedback(const humanoid_common_msgs::humanoid_follow_targetFeedbackConstPtr& feedback)
+{
+ alg_.lock();
+ //ROS_INFO("SmQrSearchAlgNode::head_follow_targetFeedback: Got Feedback!");
+
+ bool feedback_is_ok = true;
+
+ //analyze feedback
+ //my_var = feedback->var;
+
+ //if feedback is not what expected, cancel requested goal
+ if( !feedback_is_ok )
+ {
+ head_follow_target_client_.cancelGoal();
+ //ROS_INFO("SmQrSearchAlgNode::head_follow_targetFeedback: Cancelling Action!");
+ }
+ alg_.unlock();
+}
+
+
+/* [action requests] */
+bool SmQrSearchAlgNode::joint_trajectoryMakeActionRequest()
+{
+ // IMPORTANT: Please note that all mutex used in the client callback functions
+ // must be unlocked before calling any of the client class functions from an
+ // other thread (MainNodeThread).
+ // this->alg_.unlock();
+ if(joint_trajectory_client_.isServerConnected())
+ {
+ //ROS_DEBUG("SmQrSearchAlgNode::joint_trajectoryMakeActionRequest: Server is Available!");
+ //send a goal to the action server
+ //joint_trajectory_goal_.data = my_desired_goal;
+ joint_trajectory_client_.sendGoal(joint_trajectory_goal_,
+ boost::bind(&SmQrSearchAlgNode::joint_trajectoryDone, this, _1, _2),
+ boost::bind(&SmQrSearchAlgNode::joint_trajectoryActive, this),
+ boost::bind(&SmQrSearchAlgNode::joint_trajectoryFeedback, this, _1));
+ // this->alg_.lock();
+ // ROS_DEBUG("SmQrSearchAlgNode::MakeActionRequest: Goal Sent.");
+ this->executing_trajectory=true;
+ return true;
+ }
+ else
+ {
+ // this->alg_.lock();
+ ROS_DEBUG("SmQrSearchAlgNode::joint_trajectoryMakeActionRequest: HRI server is not connected");
+ return false;
+ }
+}
+
+bool SmQrSearchAlgNode::head_follow_targetMakeActionRequest()
+{
+ // IMPORTANT: Please note that all mutex used in the client callback functions
+ // must be unlocked before calling any of the client class functions from an
+ // other thread (MainNodeThread).
+ // this->alg_.unlock();
+ if(head_follow_target_client_.isServerConnected())
+ {
+ //ROS_DEBUG("SmQrSearchAlgNode::head_follow_targetMakeActionRequest: Server is Available!");
+ //send a goal to the action server
+ //head_follow_target_goal_.data = my_desired_goal;
+ head_follow_target_client_.sendGoal(head_follow_target_goal_,
+ boost::bind(&SmQrSearchAlgNode::head_follow_targetDone, this, _1, _2),
+ boost::bind(&SmQrSearchAlgNode::head_follow_targetActive, this),
+ boost::bind(&SmQrSearchAlgNode::head_follow_targetFeedback, this, _1));
+ // this->alg_.lock();
+ // ROS_DEBUG("SmQrSearchAlgNode::MakeActionRequest: Goal Sent.");
+ return true;
+ }
+ else
+ {
+ // this->alg_.lock();
+ ROS_DEBUG("SmQrSearchAlgNode::head_follow_targetMakeActionRequest: HRI server is not connected");
+ return false;
+ }
+}
+
+
+void SmQrSearchAlgNode::node_config_update(Config &config, uint32_t level)
+{
+ this->alg_.lock();
+
+ if(config.start)
+ {
+ ROS_INFO("START State Machine");
+ new_goal=true;
+ config.start=false;
+ }
+ if(config.stop)
+ {
+ ROS_INFO("STOP State Machine");
+ end=true;
+ config.stop=false;
+ }
+ if(config.load)
+ {
+ ROS_INFO("Load parameters");
+ pan_angle_init=config.pan_angle_init;
+ pan_angle_end=config.pan_angle_end;
+ tilt_angle_search=config.tilt_angle_search;
+ config.load=false;
+ }
+
+ this->config_=config;
+ this->alg_.unlock();
+}
+
+void SmQrSearchAlgNode::addNodeDiagnostics(void)
+{
+}
+
+
+//-------------------------
+bool SmQrSearchAlgNode::set_servo_module_head(std::string module)
+{
+ this->set_servo_modules_srv_.request.names.resize(2);
+ this->set_servo_modules_srv_.request.modules.resize(2);
+ this->set_servo_modules_srv_.request.names[0]="j_pan";
+ this->set_servo_modules_srv_.request.modules[0]=module;
+ this->set_servo_modules_srv_.request.names[1]="j_tilt";
+ this->set_servo_modules_srv_.request.modules[1]=module;
+ if(this->set_servo_modules_client_.call(this->set_servo_modules_srv_))
+ return true;
+ else
+ {
+ ROS_DEBUG("Impossible to call set_servo_modules service");
+ return false;
+ }
+}
+
+bool SmQrSearchAlgNode::set_servo_module(std::string module)
+{
+ unsigned int i;
+ this->set_servo_modules_srv_.request.names.resize(NUM_SERVOS);
+ this->set_servo_modules_srv_.request.modules.resize(NUM_SERVOS);
+ for(i=0;i<NUM_SERVOS;i++)
+ {
+ this->set_servo_modules_srv_.request.names[i]=servo_names[i];
+ this->set_servo_modules_srv_.request.modules[i]=module;
+ }
+ if(this->set_servo_modules_client_.call(this->set_servo_modules_srv_))
+ return true;
+ else
+ {
+ ROS_DEBUG("Impossible to call set_servo_modules service");
+ return false;
+ }
+
+}
+
+//Walk
+bool SmQrSearchAlgNode::set_walk_params(void)
+{
+ set_walk_params_srv_.request.params.Y_SWAP_AMPLITUDE = config_.Y_SWAP_AMPLITUDE;
+ set_walk_params_srv_.request.params.Z_SWAP_AMPLITUDE = config_.Z_SWAP_AMPLITUDE;
+ set_walk_params_srv_.request.params.ARM_SWING_GAIN = config_.ARM_SWING_GAIN;
+ set_walk_params_srv_.request.params.PELVIS_OFFSET = config_.PELVIS_OFFSET;
+ set_walk_params_srv_.request.params.HIP_PITCH_OFFSET = config_.HIP_PITCH_OFFSET;
+ set_walk_params_srv_.request.params.X_OFFSET = config_.X_OFFSET;
+ set_walk_params_srv_.request.params.Y_OFFSET = config_.Y_OFFSET;
+ set_walk_params_srv_.request.params.Z_OFFSET = config_.Z_OFFSET;
+ set_walk_params_srv_.request.params.A_OFFSET = config_.A_OFFSET;
+ set_walk_params_srv_.request.params.P_OFFSET = config_.P_OFFSET;
+ set_walk_params_srv_.request.params.R_OFFSET = config_.R_OFFSET;
+ set_walk_params_srv_.request.params.PERIOD_TIME = config_.PERIOD_TIME;
+ set_walk_params_srv_.request.params.DSP_RATIO = config_.DSP_RATIO;
+ set_walk_params_srv_.request.params.STEP_FB_RATIO = config_.STEP_FB_RATIO;
+ set_walk_params_srv_.request.params.FOOT_HEIGHT = config_.FOOT_HEIGHT;
+ set_walk_params_srv_.request.params.MAX_VEL = config_.MAX_VEL;
+ set_walk_params_srv_.request.params.MAX_ROT_VEL = config_.MAX_ROT_VEL;
+ ROS_INFO("SmQrSearchAlgNode:: Sending New Request!");
+ if(set_walk_params_client_.call(set_walk_params_srv_))
+ {
+ if(set_walk_params_srv_.response.ret)
+ ROS_INFO("SmQrSearchAlgNode:: Parameters changed successfully");
+ else
+ ROS_INFO("SmQrSearchAlgNode:: Impossible to change parameters");
+ }
+ else
+ ROS_INFO("SmQrSearchAlgNode:: Failed to Call Server on topic set_walk_params ");
+
+}
+
+
+void SmQrSearchAlgNode::send_cmd_vel(double x_amplitude, double y_amplitude, double a_amplitude)
+{
+ /* this->cmd_vel_Twist_msg_.linear.x=x_amplitude/period_time;
+ this->cmd_vel_Twist_msg_.linear.y=y_amplitude/period_time;
+ this->cmd_vel_Twist_msg_.linear.z=0;
+ this->cmd_vel_Twist_msg_.angular.x=0;
+ this->cmd_vel_Twist_msg_.angular.y=0;
+ this->cmd_vel_Twist_msg_.angular.z=a_amplitude/period_time;
+*/
+ this->cmd_vel_Twist_msg_.linear.x=x_amplitude;
+ this->cmd_vel_Twist_msg_.linear.y=y_amplitude;
+ this->cmd_vel_Twist_msg_.linear.z=0;
+ this->cmd_vel_Twist_msg_.angular.x=0;
+ this->cmd_vel_Twist_msg_.angular.y=0;
+ this->cmd_vel_Twist_msg_.angular.z=a_amplitude;
+
+ this->cmd_vel_publisher_.publish(this->cmd_vel_Twist_msg_);
+}
+
+//Head Tracking
+void SmQrSearchAlgNode::set_head_tracking_goal(double pan_target, double tilt_target)
+{
+ this->head_follow_target_goal_.target_pan=pan_target;
+ this->head_follow_target_goal_.target_tilt=tilt_target;
+ // this->head_follow_target_goal_.pan_range.resize(2);
+ /* this->head_follow_target_goal_.pan_range[0]=this->config_.max_pan;
+ this->head_follow_target_goal_.pan_range[1]=this->config_.min_pan;
+ this->head_follow_target_goal_.tilt_range.resize(2);
+ this->head_follow_target_goal_.tilt_range[0]=this->config_.max_tilt;
+ this->head_follow_target_goal_.tilt_range[1]=this->config_.min_tilt;
+ */
+ this->head_follow_target_goal_.pan_range[0]=1.5;
+ this->head_follow_target_goal_.pan_range[1]=-1.5;
+ //this->head_follow_target_goal_.tilt_range.resize(2);
+ this->head_follow_target_goal_.tilt_range[0]=1;
+ this->head_follow_target_goal_.tilt_range[1]=-1;
+}
+
+void SmQrSearchAlgNode::send_head_target(double pan, double tilt)
+{
+ //this->head_target_JointTrajectoryPoint_msg_.positions.resize(2);
+ this->head_target_JointTrajectoryPoint_msg_.positions[0]=pan;
+ this->head_target_JointTrajectoryPoint_msg_.positions[1]=tilt;
+ this->head_target_publisher_.publish(this->head_target_JointTrajectoryPoint_msg_);
+}
+
+//Joint trajectory
+void SmQrSearchAlgNode::joint_trajectory_set_goal(double pan_position, double tilt_position)
+{
+ /* this->move_joints_goal_.trajectory.header.seq=0;
+ this->move_joints_goal_.trajectory.header.stamp=ros::Time::now();
+ this->move_joints_goal_.trajectory.header.frame_id="MP_BODY";
+
+ this->move_joints_goal_.trajectory.joint_names.resize(2);
+ this->move_joints_goal_.trajectory.joint_names[0]="j_pan";
+ this->move_joints_goal_.trajectory.joint_names[1]="j_tilt";
+
+ this->move_joints_goal_.trajectory.points.resize(1);
+ this->move_joints_goal_.trajectory.points[0].positions.resize(2);
+ this->move_joints_goal_.trajectory.points[0].velocities.resize(2);
+ this->move_joints_goal_.trajectory.points[0].accelerations.resize(2);
+
+ this->move_joints_goal_.trajectory.points[0].positions[0]=this->current_state.position[18]*180.0/3.14159;
+ this->move_joints_goal_.trajectory.points[0].positions[1]=60.0-15.0*this->n;
+ */
+
+ joint_trajectory_goal_.trajectory.joint_names[0]="j_pan";
+ joint_trajectory_goal_.trajectory.joint_names[1]="j_tilt";
+ joint_trajectory_goal_.trajectory.points[0].positions[0]=pan_position;
+ joint_trajectory_goal_.trajectory.points[0].positions[1]=tilt_position;
+ joint_trajectory_goal_.trajectory.points[0].velocities[0]=config_.speed;
+ joint_trajectory_goal_.trajectory.points[0].velocities[1]=config_.speed;
+ joint_trajectory_goal_.trajectory.points[0].accelerations[0]=config_.acceleration;
+ joint_trajectory_goal_.trajectory.points[0].accelerations[1]=config_.acceleration;
+ //ROS_INFO("Set Joint Trajectory GOAL");
+
+}
+
+bool SmQrSearchAlgNode::joint_trajectory_start_action(void)
+{
+ if(this->joint_trajectoryMakeActionRequest())
+ {
+ /* joint_trajectory_goal_.trajectory.joint_names.clear();
+ joint_trajectory_goal_.trajectory.points[0].positions.clear();
+ joint_trajectory_goal_.trajectory.points[0].velocities.clear();
+ joint_trajectory_goal_.trajectory.points[0].accelerations.clear();
+ */
+ ROS_INFO("Sent joint trajectory action request");
+ return true;
+ }
+ else
+ {
+ ROS_INFO("Impossible to make Joint Trajectory action request!");
+ return false;
+ }
+
+}
+
+
+/* main function */
+int main(int argc,char *argv[])
+{
+ return algorithm_base::main<SmQrSearchAlgNode>(argc, argv, "sm_qr_search_alg_node");
+}
diff --git a/track_charge_station/CMakeLists.txt b/track_charge_station/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..00b6ffe839aae4e108a6d4fa9d35fe1da8a4ba49
--- /dev/null
+++ b/track_charge_station/CMakeLists.txt
@@ -0,0 +1,105 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(track_charge_station)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED)
+# ********************************************************************
+# Add catkin additional components here
+# ********************************************************************
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm sensor_msgs trajectory_msgs actionlib geometry_msgs humanoid_common_msgs iri_ros_tools)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+# ********************************************************************
+# Add system and labrobotica dependencies here
+# ********************************************************************
+# find_package(<dependency> REQUIRED)
+
+# ********************************************************************
+# Add topic, service and action definition here
+# ********************************************************************
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs # Or other packages containing msgs
+# )
+
+# ********************************************************************
+# Add the dynamic reconfigure file
+# ********************************************************************
+generate_dynamic_reconfigure_options(cfg/TrackChargeStation.cfg)
+
+# ********************************************************************
+# Add run time dependencies here
+# ********************************************************************
+catkin_package(
+# INCLUDE_DIRS
+# LIBRARIES
+# ********************************************************************
+# Add ROS and IRI ROS run time dependencies
+# ********************************************************************
+ CATKIN_DEPENDS iri_base_algorithm sensor_msgs trajectory_msgs actionlib geometry_msgs humanoid_common_msgs iri_ros_tools
+# ********************************************************************
+# Add system and labrobotica run time dependencies here
+# ********************************************************************
+# DEPENDS
+)
+
+###########
+## Build ##
+###########
+
+# ********************************************************************
+# Add the include directories
+# ********************************************************************
+include_directories(include)
+include_directories(${catkin_INCLUDE_DIRS})
+include_directories(${iriutils_INCLUDE_DIRS})
+# include_directories(${<dependency>_INCLUDE_DIR})
+
+## Declare a cpp library
+# add_library(${PROJECT_NAME} <list of source files>)
+
+## Declare a cpp executable
+add_executable(${PROJECT_NAME} src/track_charge_station_alg.cpp src/track_charge_station_alg_node.cpp)
+
+# ********************************************************************
+# Add the libraries
+# ********************************************************************
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+# target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
+
+# ********************************************************************
+# Add message headers dependencies
+# ********************************************************************
+# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} sensor_msgs_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} trajectory_msgs_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} geometry_msgs_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} humanoid_common_msgs_generate_messages_cpp)
+# ********************************************************************
+# Add dynamic reconfigure dependencies
+# ********************************************************************
+add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
diff --git a/track_charge_station/cfg/TrackChargeStation.cfg b/track_charge_station/cfg/TrackChargeStation.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..3f7f1e0a84c321f2bd2b20d03687512c66c737b2
--- /dev/null
+++ b/track_charge_station/cfg/TrackChargeStation.cfg
@@ -0,0 +1,87 @@
+#! /usr/bin/env python
+#* All rights reserved.
+#*
+#* Redistribution and use in source and binary forms, with or without
+#* modification, are permitted provided that the following conditions
+#* are met:
+#*
+#* * Redistributions of source code must retain the above copyright
+#* notice, this list of conditions and the following disclaimer.
+#* * Redistributions in binary form must reproduce the above
+#* copyright notice, this list of conditions and the following
+#* disclaimer in the documentation and/or other materials provided
+#* with the distribution.
+#* * Neither the name of the Willow Garage nor the names of its
+#* contributors may be used to endorse or promote products derived
+#* from this software without specific prior written permission.
+#*
+#* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#* POSSIBILITY OF SUCH DAMAGE.
+#***********************************************************
+
+# Author:
+
+PACKAGE='track_charge_station'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# Name Type Reconfiguration level Description Default Min Max
+#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
+gen.add("start", bool_t, 0, "Start state machine", False)
+gen.add("stop", bool_t, 0, "Stop state machine", False)
+gen.add("load", bool_t, 0, "Load pid parameters", False)
+gen.add("z_target", double_t, 0, "Z target distance to charge station", 20, 10, 30)
+gen.add("x_target", double_t, 0, "X target distance to charge station", 20, 10, 30)
+gen.add("yaw_target", double_t, 0, "Orientation target to charge station", 20, 10, 30)
+gen.add("z_kp", double_t, 0, "Desired kp for z distance", 0.1, 0.0, 0.28)
+gen.add("z_ki", double_t, 0, "Desired ki", 0.0, 0.0, 0.28)
+gen.add("z_kd", double_t, 0, "Desired kd", 0.0, 0.0, 0.28)
+gen.add("period", double_t, 0, "Period PID", 1.0, 0.0, 100)
+gen.add("z_tolerance", double_t, 0, "Tolerance in distance z", 1.0, 0.0, 10)
+#Walk
+gen.add("Y_SWAP_AMPLITUDE", double_t, 0, "Y swap amplitude (m)", 0.025, 0, 0.050)
+gen.add("Z_SWAP_AMPLITUDE", double_t, 0, "X swap amplitude (m)", 0.005, 0, 0.050)
+gen.add("ARM_SWING_GAIN", double_t, 0, "Arm swing gain", 1.5, 0, 10.0)
+gen.add("PELVIS_OFFSET", double_t, 0, "Pelvis offset (rad)", 0.052, 0, 0.52)
+gen.add("HIP_PITCH_OFFSET", double_t, 0, "Hip pitch offset (rad)", 0.174, 0, 0.52)
+gen.add("X_OFFSET", double_t, 0, "X offset (m)", -0.025, -0.050, 0.050)
+gen.add("Y_OFFSET", double_t, 0, "Y offset (m)", 0.0075, -0.050, 0.050)
+gen.add("Z_OFFSET", double_t, 0, "Z offset (m)", 0.035, -0.050, 0.050)
+gen.add("A_OFFSET", double_t, 0, "A offset (rad)", 0, -0.52, 0.52)
+gen.add("P_OFFSET", double_t, 0, "P offset (rad)", 0, -0.52, 0.52)
+gen.add("R_OFFSET", double_t, 0, "R offset (rad)", 0, -0.52, 0.52)
+gen.add("PERIOD_TIME", double_t, 0, "Period (s)", 0.6, 0, 1.0)
+gen.add("DSP_RATIO", double_t, 0, "Double support ratio", 0.1, 0, 1.0)
+gen.add("STEP_FB_RATIO", double_t, 0, "Step forward/backward ratio", 0.3, 0, 1.0)
+gen.add("FOOT_HEIGHT", double_t, 0, "Foot height (m)", 0.04, 0, 0.050)
+gen.add("MAX_VEL", double_t, 0, "Maximum linear velocity (m/s)", 0.01, 0, 1.0)
+gen.add("MAX_ROT_VEL", double_t, 0, "Maximum rotational velocity (rad/s)", 0.01, 0, 1.0)
+#Head tracking
+gen.add("pan_p", double_t, 0, "Desired pan kp", 0.1, 0.0, 0.28)
+gen.add("pan_i", double_t, 0, "Desired pan ki", 0.0, 0.0, 0.28)
+gen.add("pan_d", double_t, 0, "Desired pan kd", 0.0, 0.0, 0.28)
+gen.add("update_pan_pid", bool_t, 0, "Update pan pid", False)
+gen.add("pan_i_clamp", double_t, 0, "Desired pan integral limit", 0.0, 0.0, 0.28)
+gen.add("max_pan", double_t, 0, "Maximum pan angle", 3.14159, -3.14159, 3.14159)
+gen.add("min_pan", double_t, 0, "Minimum pan angle", -3.14159, -3.14159, 3.14159)
+gen.add("tilt_p", double_t, 0, "Desired tilt kp", 0.1, 0.0, 0.28)
+gen.add("tilt_i", double_t, 0, "Desired tilt ki", 0.0, 0.0, 0.28)
+gen.add("tilt_d", double_t, 0, "Desired tilt kd", 0.0, 0.0, 0.28)
+gen.add("update_tilt_pid", bool_t, 0, "Update tilt pid", False)
+gen.add("tilt_i_clamp", double_t, 0, "Desired tilt integral limit", 0.0, 0.0, 0.28)
+gen.add("max_tilt", double_t, 0, "Maximum tilt angle", 3.14159, -3.14159, 3.14159)
+gen.add("min_tilt", double_t, 0, "Minimum tilt angle", -3.14159, -3.14159, 3.14159)
+
+exit(gen.generate(PACKAGE, "TrackChargeStationAlgorithm", "TrackChargeStation"))
diff --git a/track_charge_station/include/track_charge_station_alg.h b/track_charge_station/include/track_charge_station_alg.h
new file mode 100644
index 0000000000000000000000000000000000000000..76d9fb2b08b277d69a80605a048a4fd3c86a2cec
--- /dev/null
+++ b/track_charge_station/include/track_charge_station_alg.h
@@ -0,0 +1,131 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+// IMPORTANT NOTE: This code has been generated through a script from the
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _track_charge_station_alg_h_
+#define _track_charge_station_alg_h_
+
+#include <track_charge_station/TrackChargeStationConfig.h>
+
+//include track_charge_station_alg main library
+
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ *
+ */
+class TrackChargeStationAlgorithm
+{
+ protected:
+ /**
+ * \brief define config type
+ *
+ * Define a Config type with the TrackChargeStationConfig. All driver implementations
+ * will then use the same variable type Config.
+ */
+ pthread_mutex_t access_;
+
+ // private attributes and methods
+
+ public:
+ /**
+ * \brief define config type
+ *
+ * Define a Config type with the TrackChargeStationConfig. All driver implementations
+ * will then use the same variable type Config.
+ */
+ typedef track_charge_station::TrackChargeStationConfig Config;
+
+ /**
+ * \brief config variable
+ *
+ * This variable has all the driver parameters defined in the cfg config file.
+ * Is updated everytime function config_update() is called.
+ */
+ Config config_;
+
+ /**
+ * \brief constructor
+ *
+ * In this constructor parameters related to the specific driver can be
+ * initalized. Those parameters can be also set in the openDriver() function.
+ * Attributes from the main node driver class IriBaseDriver such as loop_rate,
+ * may be also overload here.
+ */
+ TrackChargeStationAlgorithm(void);
+
+ /**
+ * \brief Lock Algorithm
+ *
+ * Locks access to the Algorithm class
+ */
+ void lock(void) { pthread_mutex_lock(&this->access_); };
+
+ /**
+ * \brief Unlock Algorithm
+ *
+ * Unlocks access to the Algorithm class
+ */
+ void unlock(void) { pthread_mutex_unlock(&this->access_); };
+
+ /**
+ * \brief Tries Access to Algorithm
+ *
+ * Tries access to Algorithm
+ *
+ * \return true if the lock was adquired, false otherwise
+ */
+ bool try_enter(void)
+ {
+ if(pthread_mutex_trylock(&this->access_)==0)
+ return true;
+ else
+ return false;
+ };
+
+ /**
+ * \brief config update
+ *
+ * In this function the driver parameters must be updated with the input
+ * config variable. Then the new configuration state will be stored in the
+ * Config attribute.
+ *
+ * \param new_cfg the new driver configuration state
+ *
+ * \param level level in which the update is taken place
+ */
+ void config_update(Config& config, uint32_t level=0);
+
+ // here define all track_charge_station_alg interface methods to retrieve and set
+ // the driver parameters
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor is called when the object is about to be destroyed.
+ *
+ */
+ ~TrackChargeStationAlgorithm(void);
+};
+
+#endif
diff --git a/track_charge_station/include/track_charge_station_alg_node.h b/track_charge_station/include/track_charge_station_alg_node.h
new file mode 100644
index 0000000000000000000000000000000000000000..7db52a1109280b3f74212c50b74f0aee9043cb25
--- /dev/null
+++ b/track_charge_station/include/track_charge_station_alg_node.h
@@ -0,0 +1,244 @@
+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+// IMPORTANT NOTE: This code has been generated through a script from the
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _track_charge_station_alg_node_h_
+#define _track_charge_station_alg_node_h_
+
+#include <iri_ros_tools/watchdog.h>
+#include <iri_base_algorithm/iri_base_algorithm.h>
+#include "track_charge_station_alg.h"
+
+// [publisher subscriber headers]
+#include <sensor_msgs/JointState.h>
+#include <trajectory_msgs/JointTrajectoryPoint.h>
+#include <geometry_msgs/Twist.h>
+#include <humanoid_common_msgs/tag_pose_array.h>
+
+// [service client headers]
+#include <humanoid_common_msgs/set_servo_modules.h>
+#include <humanoid_common_msgs/set_pid.h>
+#include <humanoid_common_msgs/set_walk_params.h>
+
+// [action server client headers]
+#include <actionlib/client/simple_action_client.h>
+#include <actionlib/client/terminal_state.h>
+#include <humanoid_common_msgs/humanoid_follow_targetAction.h>
+
+
+//states
+typedef enum {IDLE, QR, WALK, SIT} States;
+
+//PID
+typedef struct {double kp;
+ double ki;
+ double kd;
+ double error;
+ double prev_error;
+ double integral;
+ double derivative;
+ double target;
+ double period;
+ }TPID;
+
+/**
+ * \brief IRI ROS Specific Algorithm Class
+ *
+ */
+class TrackChargeStationAlgNode : public algorithm_base::IriBaseAlgorithm<TrackChargeStationAlgorithm>
+{
+ private:
+ // [publisher attributes]
+ ros::Publisher head_target_publisher_;
+ trajectory_msgs::JointTrajectoryPoint head_target_JointTrajectoryPoint_msg_;
+
+ ros::Publisher cmd_vel_publisher_;
+ geometry_msgs::Twist cmd_vel_Twist_msg_;
+
+
+ // [subscriber attributes]
+ ros::Subscriber joint_states_subscriber_;
+ void joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg);
+ pthread_mutex_t joint_states_mutex_;
+ void joint_states_mutex_enter(void);
+ void joint_states_mutex_exit(void);
+
+ ros::Subscriber qr_pose_subscriber_;
+ void qr_pose_callback(const humanoid_common_msgs::tag_pose_array::ConstPtr& msg);
+ pthread_mutex_t qr_pose_mutex_;
+ void qr_pose_mutex_enter(void);
+ void qr_pose_mutex_exit(void);
+
+
+ // [service attributes]
+
+ // [client attributes]
+ ros::ServiceClient set_servo_modules_client_;
+ humanoid_common_msgs::set_servo_modules set_servo_modules_srv_;
+
+ ros::ServiceClient set_pan_pid_client_;
+ humanoid_common_msgs::set_pid set_pan_pid_srv_;
+
+ ros::ServiceClient set_walk_params_client_;
+ humanoid_common_msgs::set_walk_params set_walk_params_srv_;
+
+
+ // [action server attributes]
+
+ // [action client attributes]
+ actionlib::SimpleActionClient<humanoid_common_msgs::humanoid_follow_targetAction> head_follow_target_client_;
+ humanoid_common_msgs::humanoid_follow_targetGoal head_follow_target_goal_;
+ bool head_follow_targetMakeActionRequest();
+ void head_follow_targetDone(const actionlib::SimpleClientGoalState& state, const humanoid_common_msgs::humanoid_follow_targetResultConstPtr& result);
+ void head_follow_targetActive();
+ void head_follow_targetFeedback(const humanoid_common_msgs::humanoid_follow_targetFeedbackConstPtr& feedback);
+
+
+ States state;
+ bool new_goal;
+ bool end;
+ bool executing_trajectory;
+ //Head tracking
+ double current_pan_angle;
+ double current_tilt_angle;
+ double next_pan_angle;
+ double next_tilt_angle;
+ double pan_angle_init;
+ double pan_angle_end;
+ double tilt_angle_search;
+ //qr
+ double pan_angle;
+ double tilt_angle;
+ bool tracking;
+ bool qr_detected;
+ unsigned int counter;
+ double qr_position_z;
+ double qr_position_x;
+ double qr_position_y;
+ CROSWatchdog watchdog_qr_lost;
+ //PID
+ TPID z_distance;
+ TPID x_distance;
+ TPID orientation;
+ double pid;
+ double pid_out;
+ double z_distance_target;
+ ros::Time start_time;
+ ros::Time new_time;
+ //ros::Duration time_period;
+ double time_period;
+ //walk
+ //añadir a la estructura TPID?
+ double cmd_vel_x;
+ double cmd_vel_y; //z_distance
+ double cmd_vel_a;
+ double z_error;
+ double x_error;
+ double rot_error;
+ double z_min_tolerance;
+
+
+
+
+ /**
+ * \brief config variable
+ *
+ * This variable has all the driver parameters defined in the cfg config file.
+ * Is updated everytime function config_update() is called.
+ */
+ Config config_;
+ public:
+ /**
+ * \brief Constructor
+ *
+ * This constructor initializes specific class attributes and all ROS
+ * communications variables to enable message exchange.
+ */
+ TrackChargeStationAlgNode(void);
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor frees all necessary dynamic memory allocated within this
+ * this class.
+ */
+ ~TrackChargeStationAlgNode(void);
+
+ protected:
+ /**
+ * \brief main node thread
+ *
+ * This is the main thread node function. Code written here will be executed
+ * in every node loop while the algorithm is on running state. Loop frequency
+ * can be tuned by modifying loop_rate attribute.
+ *
+ * Here data related to the process loop or to ROS topics (mainly data structs
+ * related to the MSG and SRV files) must be updated. ROS publisher objects
+ * must publish their data in this process. ROS client servers may also
+ * request data to the corresponding server topics.
+ */
+ void mainNodeThread(void);
+
+ /**
+ * \brief dynamic reconfigure server callback
+ *
+ * This method is called whenever a new configuration is received through
+ * the dynamic reconfigure. The derivated generic algorithm class must
+ * implement it.
+ *
+ * \param config an object with new configuration from all algorithm
+ * parameters defined in the config file.
+ * \param level integer referring the level in which the configuration
+ * has been changed.
+ */
+ void node_config_update(Config &config, uint32_t level);
+
+ /**
+ * \brief node add diagnostics
+ *
+ * In this abstract function additional ROS diagnostics applied to the
+ * specific algorithms may be added.
+ */
+ void addNodeDiagnostics(void);
+
+ // [diagnostic functions]
+
+ // [test functions]
+
+ void start_tracking(double z_target, double x_target, double yaw_target);
+ bool set_servo_module_head(std::string module);
+ bool set_servo_module(std::string module);
+ //Head tracking
+ void set_head_tracking_goal(double pan_target, double tilt_target);
+ void send_head_target(double pan, double tilt);
+ //Walk
+ bool set_walk_params(void);
+ void send_cmd_vel(double x_amplitude, double y_amplitude, double a_amplitude);
+ void stop_walking(void);
+ //PID
+ // void pid(void);
+ // double pid(void);
+ void pid2(TPID caca, double &culo);
+};
+
+#endif
diff --git a/track_charge_station/launch/sim_track_charge_station.launch b/track_charge_station/launch/sim_track_charge_station.launch
new file mode 100644
index 0000000000000000000000000000000000000000..d1f8966687bb307c164b517e611d60249a826b1f
--- /dev/null
+++ b/track_charge_station/launch/sim_track_charge_station.launch
@@ -0,0 +1,70 @@
+
+<launch>
+
+ <arg name="robot" default="darwin" />
+ <arg name="environment" default="vision_env" />
+
+ <include file="$(find darwin_description)/launch/darwin_sim.launch">
+ <arg name="robot" value="$(arg robot)" />
+ </include>
+
+ <include file="$(find bioloid_description)/launch/vision_env.launch">
+ <arg name="environment" value="$(arg environment)" />
+ </include>
+
+
+ <!-- launch the QR head tracking node -->
+ <node name="track_charge_station"
+ pkg="track_charge_station"
+ type="track_charge_station"
+ output="screen"
+ ns="/darwin">
+ <remap from="/darwin/track_charge_station/cmd_vel"
+ to="/darwin/robot/cmd_vel"/>
+ <remap from="/darwin/track_charge_station/set_pan_pid"
+ to="/darwin/robot/set_pan_pid"/>
+ <remap from="/darwin/track_charge_station/set_walk_params"
+ to="/darwin/robot/set_walk_params"/>
+ <remap from="/darwin/track_charge_station/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/head_follow_target"
+ to="/darwin/robot/head_follow_target"/>
+ <remap from="/darwin/track_charge_station/head_target"
+ to="/darwin/robot/head_target"/>
+ <remap from="/darwin/track_charge_station/joint_states"
+ to="/darwin/joint_states"/>
+ <!-- <remap from="/darwin/track_charge_station/qr_pose"
+ to="/qr_detector/qr_pose"/> -->
+ </node>
+
+<!-- <include file="$(find qr_detector)/launch/tracker_darwin.launch"/> -->
+
+ <node pkg="qr_detector"
+ name="qr_detector"
+ type="qr_detector"
+ output="screen"
+ ns="/darwin">
+ <param name="qr_x" value="0.12"/>
+ <param name="qr_y" value="0.12"/>
+ <param name="camera_frame" value="/darwin/camera_link"/>
+ <remap from="/darwin/qr_detector/camera/image_raw"
+ to="/darwin/camera/image_raw"/>
+ <remap from="/darwin/qr_detector/camera/camera_info"
+ to="/darwin/camera/camera_info"/>
+ <remap from="/darwin/qr_detector/qr_pose"
+ to="/darwin/track_charge_station/qr_pose"/>
+ </node>
+
+
+ <node pkg="image_view"
+ name="image_view"
+ type="image_view"
+ args="image:=/darwin/usb_cam/image_raw">
+ </node>
+
+ <!-- launch dynamic reconfigure -->
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+ output="screen"/>
+
+
+</launch>
\ No newline at end of file
diff --git a/track_charge_station/launch/track_charge_station.launch b/track_charge_station/launch/track_charge_station.launch
new file mode 100644
index 0000000000000000000000000000000000000000..5b99336aec319587e68a2d434d9015719f8c4cf3
--- /dev/null
+++ b/track_charge_station/launch/track_charge_station.launch
@@ -0,0 +1,72 @@
+<launch>
+
+ <arg name="robot" default="darwin" />
+
+ <include file="$(find darwin_description)/launch/darwin_base.launch">
+ <arg name="robot" value="$(arg robot)" />
+ </include>
+
+ <!-- launch the QR head tracking node -->
+ <node name="track_charge_station"
+ pkg="track_charge_station"
+ type="track_charge_station"
+ output="screen"
+ ns="/darwin">
+<!-- <param name="load" value="false"/>
+ <param name="pan_angle_init" value="-1.5"/>
+ <param name="pan_angle_end" value="1.5"/>
+ <param name="tilt_angle_search" value="0.8"/>
+ <param name="speed" value="1"/>
+ <param name="acceleration" value="0.3"/>
+
+ <param name="Y_SWAP_AMPLITUDE" value="0.020"/>
+ <param name="Z_SWAP_AMPLITUDE" value="0.005"/>
+ <param name="ARM_SWING_GAIN" value="1.5"/>
+ <param name="PELVIS_OFFSET" value="0.05"/>
+ <param name="HIP_PITCH_OFFSET" value="0.23"/>
+ <param name="X_OFFSET" value="-0.01"/>
+ <param name="Y_OFFSET" value="0.005"/>
+ <param name="Z_OFFSET" value="0.02"/>
+ <param name="A_OFFSET" value="0.0"/>
+ <param name="P_OFFSET" value="0.0"/>
+ <param name="R_OFFSET" value="0.0"/>
+ <param name="PERIOD_TIME" value="0.6"/>
+ <param name="DSP_RATIO" value="0.1"/>
+ <param name="STEP_FB_RATIO" value="0.28"/>
+ <param name="FOOT_HEIGHT" value="0.04"/>
+ <param name="MAX_VEL" value="0.01"/>
+ <param name="MAX_ROT_VEL" value="0.01"/> -->
+
+ <remap from="/darwin/track_charge_station/cmd_vel"
+ to="/darwin/robot/cmd_vel"/>
+ <remap from="/darwin/track_charge_station/set_pan_pid"
+ to="/darwin/robot/set_pan_pid"/>
+ <remap from="/darwin/track_charge_station/set_walk_params"
+ to="/darwin/robot/set_walk_params"/>
+ <remap from="/darwin/track_charge_station/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/head_follow_target"
+ to="/darwin/robot/head_follow_target"/>
+ <remap from="/darwin/track_charge_station/head_target"
+ to="/darwin/robot/head_target"/>
+ <remap from="/darwin/track_charge_station/joint_states"
+ to="/darwin/joint_states"/>
+ <remap from="/darwin/track_charge_station/qr_pose"
+ to="/qr_detector/qr_pose"/>
+ </node>
+
+<!-- <include file="$(find darwin_apps)/launch/xs_camera_qr.launch"/>-->
+ <include file="$(find qr_detector)/launch/tracker_darwin.launch"/>
+
+ <node pkg="image_view"
+ name="image_view"
+ type="image_view"
+ args="image:=/darwin/usb_cam/image_raw">
+ </node>
+
+ <!-- launch dynamic reconfigure -->
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+ output="screen"/>
+
+
+</launch>
\ No newline at end of file
diff --git a/track_charge_station/package.xml b/track_charge_station/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..170d809c8e74bad4b03e1dcfa260b3ca4dfa20ee
--- /dev/null
+++ b/track_charge_station/package.xml
@@ -0,0 +1,64 @@
+<?xml version="1.0"?>
+<package>
+ <name>track_charge_station</name>
+ <version>0.0.0</version>
+ <description>The track_charge_station package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="igarcia@todo.todo">igarcia</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>LGPL</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/track_charge_station</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>iri_base_algorithm</build_depend>
+ <build_depend>sensor_msgs</build_depend>
+ <build_depend>trajectory_msgs</build_depend>
+ <build_depend>actionlib</build_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_depend>humanoid_common_msgs</build_depend>
+ <build_depend>iri_ros_tools</build_depend>
+ <run_depend>iri_base_algorithm</run_depend>
+ <run_depend>sensor_msgs</run_depend>
+ <run_depend>trajectory_msgs</run_depend>
+ <run_depend>actionlib</run_depend>
+ <run_depend>geometry_msgs</run_depend>
+ <run_depend>humanoid_common_msgs</run_depend>
+ <run_depend>iri_ros_tools</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/track_charge_station/src/track_charge_station_alg.cpp b/track_charge_station/src/track_charge_station_alg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c609c8a104383c6256a1603215e8c3a49b5f355b
--- /dev/null
+++ b/track_charge_station/src/track_charge_station_alg.cpp
@@ -0,0 +1,23 @@
+#include "track_charge_station_alg.h"
+
+TrackChargeStationAlgorithm::TrackChargeStationAlgorithm(void)
+{
+ pthread_mutex_init(&this->access_,NULL);
+}
+
+TrackChargeStationAlgorithm::~TrackChargeStationAlgorithm(void)
+{
+ pthread_mutex_destroy(&this->access_);
+}
+
+void TrackChargeStationAlgorithm::config_update(Config& config, uint32_t level)
+{
+ this->lock();
+
+ // save the current configuration
+ this->config_=config;
+
+ this->unlock();
+}
+
+// TrackChargeStationAlgorithm Public API
diff --git a/track_charge_station/src/track_charge_station_alg_node.cpp b/track_charge_station/src/track_charge_station_alg_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ac68f8ee3ad8fd101ff1c6361090f2b924e50cc2
--- /dev/null
+++ b/track_charge_station/src/track_charge_station_alg_node.cpp
@@ -0,0 +1,710 @@
+#include "track_charge_station_alg_node.h"
+
+#define NUM_SERVOS 32
+// joint names
+const std::string servo_names[NUM_SERVOS]={std::string("Servo0"),
+ std::string("j_shoulder_pitch_r"),
+ std::string("j_shoulder_pitch_l"),
+ std::string("j_shoulder_roll_r"),
+ std::string("j_shoulder_roll_l"),
+ std::string("j_elbow_r"),
+ std::string("j_elbow_l"),
+ std::string("j_hip_yaw_r"),
+ std::string("j_hip_yaw_l"),
+ std::string("j_hip_roll_r"),
+ std::string("j_hip_roll_l"),
+ std::string("j_hip_pitch_r"),
+ std::string("j_hip_pitch_l"),
+ std::string("j_knee_r"),
+ std::string("j_knee_l"),
+ std::string("j_ankle_pitch_r"),
+ std::string("j_ankle_pitch_l"),
+ std::string("j_ankle_roll_r"),
+ std::string("j_ankle_roll_l"),
+ std::string("j_pan"),
+ std::string("j_tilt"),
+ std::string("Servo21"),
+ std::string("Servo22"),
+ std::string("Servo23"),
+ std::string("Servo24"),
+ std::string("Servo25"),
+ std::string("Servo26"),
+ std::string("Servo27"),
+ std::string("Servo28"),
+ std::string("Servo29"),
+ std::string("Servo30"),
+ std::string("Servo31")};
+
+TrackChargeStationAlgNode::TrackChargeStationAlgNode(void) :
+ algorithm_base::IriBaseAlgorithm<TrackChargeStationAlgorithm>(),
+ head_follow_target_client_("head_follow_target", true)
+{
+ //init class attributes if necessary
+ //this->loop_rate_ = 2;//in [Hz]
+
+ // [init publishers]
+ this->head_target_publisher_ = this->public_node_handle_.advertise<trajectory_msgs::JointTrajectoryPoint>("head_target", 1);
+ this->cmd_vel_publisher_ = this->public_node_handle_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+
+ // [init subscribers]
+ this->joint_states_subscriber_ = this->public_node_handle_.subscribe("joint_states", 1, &TrackChargeStationAlgNode::joint_states_callback, this);
+ pthread_mutex_init(&this->joint_states_mutex_,NULL);
+
+ this->qr_pose_subscriber_ = this->public_node_handle_.subscribe("qr_pose", 1, &TrackChargeStationAlgNode::qr_pose_callback, this);
+ pthread_mutex_init(&this->qr_pose_mutex_,NULL);
+
+
+ // [init services]
+
+ // [init clients]
+ set_servo_modules_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_servo_modules>("set_servo_modules");
+
+ set_pan_pid_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_pid>("set_pan_pid");
+
+ set_walk_params_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_walk_params>("set_walk_params");
+
+
+ // [init action servers]
+
+ // [init action clients]
+
+ state=IDLE;
+ new_goal=false;
+ //Head tracking
+ current_pan_angle=0.0;
+ current_tilt_angle=0.0;
+ //QR
+ counter=0;
+ //PID
+ pid=0.0;
+ pid_out=0.0;
+ z_distance.error=0.0;
+ z_distance.prev_error=0.0;
+ z_distance.kp=0.0;
+ z_distance.ki=0.0;
+ z_distance.kd=0.0;
+ z_distance.period=0.0;
+ //walk
+ cmd_vel_x=0.0;
+ cmd_vel_y=0.0;
+ cmd_vel_a=0.0;
+ z_error=0.0;
+ x_error=0.0;
+ rot_error=0.0;
+
+
+
+ //head_follow
+ this->head_follow_target_goal_.pan_range.resize(2);
+ this->head_follow_target_goal_.tilt_range.resize(2);
+
+ //head_target
+ this->head_target_JointTrajectoryPoint_msg_.positions.resize(2);
+ //this->head_target_JointTrajectoryPoint_msg_.velocities.resize(2);
+}
+
+TrackChargeStationAlgNode::~TrackChargeStationAlgNode(void)
+{
+ // [free dynamic memory]
+ pthread_mutex_destroy(&this->joint_states_mutex_);
+ pthread_mutex_destroy(&this->qr_pose_mutex_);
+}
+
+void TrackChargeStationAlgNode::mainNodeThread(void)
+{
+ /* // [fill msg structures]
+ // Initialize the topic message structure
+ //this->head_target_JointTrajectoryPoint_msg_.data = my_var;
+
+ // Initialize the topic message structure
+ //this->cmd_vel_Twist_msg_.data = my_var;
+
+
+ // [fill srv structure and make request to the server]
+ //set_servo_modules_srv_.request.data = my_var;
+ //ROS_INFO("TrackChargeStationAlgNode:: Sending New Request!");
+ //if (set_servo_modules_client_.call(set_servo_modules_srv_))
+ //{
+ //ROS_INFO("TrackChargeStationAlgNode:: Response: %s", set_servo_modules_srv_.response.result);
+ //}
+ //else
+ //{
+ //ROS_INFO("TrackChargeStationAlgNode:: Failed to Call Server on topic set_servo_modules ");
+ //}
+
+
+ //set_pan_pid_srv_.request.data = my_var;
+ //ROS_INFO("TrackChargeStationAlgNode:: Sending New Request!");
+ //if (set_pan_pid_client_.call(set_pan_pid_srv_))
+ //{
+ //ROS_INFO("TrackChargeStationAlgNode:: Response: %s", set_pan_pid_srv_.response.result);
+ //}
+ //else
+ //{
+ //ROS_INFO("TrackChargeStationAlgNode:: Failed to Call Server on topic set_pan_pid ");
+ //}
+
+
+ //set_walk_params_srv_.request.data = my_var;
+ //ROS_INFO("TrackChargeStationAlgNode:: Sending New Request!");
+ //if (set_walk_params_client_.call(set_walk_params_srv_))
+ //{
+ //ROS_INFO("TrackChargeStationAlgNode:: Response: %s", set_walk_params_srv_.response.result);
+ //}
+ //else
+ //{
+ //ROS_INFO("TrackChargeStationAlgNode:: Failed to Call Server on topic set_walk_params ");
+ //}
+
+
+
+ // [fill action structure and make request to the action server]
+ // variable to hold the state of the current goal on the server
+ //actionlib::SimpleClientGoalState head_follow_target_state(actionlib::SimpleClientGoalState::PENDING);
+ // to get the state of the current goal
+ //alg_.unlock();
+ //head_follow_target_state=head_follow_target_client_.getState();
+ // Possible state values are: PENDING,ACTIVE,RECALLED,REJECTED,PREEMPTED,ABORTED,SUCCEEDED and LOST
+ //alg_.lock();
+ //if(head_follow_target_state==actionlib::SimpleClientGoalState::ABORTED)
+ //{
+ // do something
+ //}
+ //else if(head_follow_target_state==actionlib::SimpleClientGoalState::SUCCEEDED)
+ //{
+ // do something else
+ //}
+ //head_follow_targetMakeActionRequest();
+
+ // IMPORTANT: Please note that all mutex used in the client callback functions
+ // must be unlocked before calling any of the client class functions from an
+ // other thread (MainNodeThread).
+
+
+ // [publish messages]
+ // Uncomment the following line to publish the topic message
+ //this->head_target_publisher_.publish(this->head_target_JointTrajectoryPoint_msg_);
+
+ // Uncomment the following line to publish the topic message
+ //this->cmd_vel_publisher_.publish(this->cmd_vel_Twist_msg_);
+*/
+
+ switch(state)
+ {
+ case IDLE:
+ ROS_INFO("IDLE");
+ if(new_goal)
+ {
+ //Start tracking QR
+ this->tracking=true;
+ set_servo_module("walk"); //Servos to walk module
+ //set_walk_params();
+ set_servo_module_head("head"); //Head servos to head module
+ set_head_tracking_goal(this->current_pan_angle, this->current_tilt_angle);
+ if(head_follow_targetMakeActionRequest())
+ {
+ state=QR;
+ }
+ new_goal=false;
+ //state=START;
+ }
+
+ break;
+
+ /* case START:
+ ROS_INFO("START");
+ pid();
+ if(end)
+ state=IDLE;
+
+ break; */
+
+ //Comprobar si se ha perdido QR y actualizar pid (con señal bool)
+ case QR:
+ // if(counter>50)
+ if(this->watchdog_qr_lost.is_active())
+ {
+ ROS_INFO("QR lost");
+ this->tracking=false;
+ this->head_follow_target_client_.cancelGoal();
+ //Volver a la maquina de estados SmQrSearchAlgNode
+ state=IDLE;
+ }
+ /* else if(end){
+ ROS_INFO("Stop State machine");
+ this->tracking=false;
+ this->head_follow_target_client_.cancelGoal();
+ end=false;
+ state=IDLE;
+ } */
+ else
+ {
+ ROS_INFO("TRACKING");
+ //counter=0;
+ //pid();
+ //send_cmd_vel(pid_out);
+ state=WALK;
+ }
+ break;
+
+ case WALK:
+ ROS_INFO("Walking z");
+ //Distancia z estacion carga
+// pid(z_distance, &z_error, &z_cmd_vel); //Actualizar PID
+// pid(x_distance, &x_error, &x_cmd_vel);
+// if(z_error<z_min_tolerance) //Si el error de la distancia z es menor para ese cmd_vel (y)
+// z_cmd_vel=0.0; //mantener los otros cmd_vel pero parar el de la direccion y
+// if(x_error<x_desired)
+// x_cmd_vel=0.0;
+// if(z_cmd_vel==0.0 && x_cmd_vel == 0.0)
+// {
+// stop_walking();
+// state=SIT;
+// }
+// else
+// {
+// send_cmd_vel(cmd_vel_x, cmd_vel_y, cmd_vel_a); //mantener los otros cmd_vel pero actualizar el de la direccion y
+// }
+
+
+ pid2(z_distance, cmd_vel_y);
+ if(z_distance.error<z_min_tolerance) //si la distancia z (error) es menor a la minima distancia admitida (tolerancia)
+ {
+ cmd_vel_y=0.0; //No mover eje y
+ state=SIT;
+ }
+ else
+ {
+ state=QR;
+ }
+ send_cmd_vel(0, cmd_vel_y, 0);
+ break;
+
+ case SIT:
+ ROS_INFO("Sitting");
+ state=IDLE;
+ break;
+
+
+}
+}
+
+/* [subscriber callbacks] */
+void TrackChargeStationAlgNode::joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg)
+{
+ //ROS_INFO("TrackChargeStationAlgNode::joint_states_callback: New Message Received");
+
+ //use appropiate mutex to shared variables if necessary
+ //this->alg_.lock();
+ this->joint_states_mutex_enter();
+
+ unsigned int i;
+
+ for (i = 0; i < msg->name.size() ; ++i){
+ if (msg->name[i]=="j_pan"){
+ this->current_pan_angle=msg->position[i];
+ //this->next_pan_angle=current_pan_angle+0.5;
+ //if(current_pan_angle>1.0)
+ // stop_search=true;
+ }
+ else if (msg->name[i]=="j_tilt"){
+ this->current_tilt_angle=(msg->position[i]);
+ }
+ }
+
+ //unlock previously blocked shared variables
+ //this->alg_.unlock();
+ this->joint_states_mutex_exit();
+}
+
+void TrackChargeStationAlgNode::joint_states_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->joint_states_mutex_);
+}
+
+void TrackChargeStationAlgNode::joint_states_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->joint_states_mutex_);
+}
+
+void TrackChargeStationAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_pose_array::ConstPtr& msg)
+{
+ //ROS_INFO("TrackChargeStationAlgNode::qr_pose_callback: New Message Received");
+
+ //use appropiate mutex to shared variables if necessary
+ //this->alg_.lock();
+ this->qr_pose_mutex_enter();
+
+
+ if(msg->tags.size()>0)
+ {
+ //qr_detected=msg->tags.size();
+ qr_detected=true;
+ qr_position_z=(msg->tags[0].position.z)*100;
+ qr_position_x=(msg->tags[0].position.x)*100;
+ qr_position_y=(msg->tags[0].position.y)*100;
+
+ this->pan_angle=this->current_pan_angle+atan2(-msg->tags[0].position.x,msg->tags[0].position.z);
+ this->tilt_angle=this->current_tilt_angle+atan2(-msg->tags[0].position.y,msg->tags[0].position.z);
+ //ROS_INFO("Next target pan angle: %f (%f,%f)",this->pan_angle,this->current_pan_angle,atan2(msg->tags[0].position.x,msg->tags[0].position.z));
+ //ROS_INFO("Next target tilt angle: %f (%f,%f)",this->tilt_angle,this->current_tilt_angle,atan2(msg->tags[0].position.y,msg->tags[0].position.z));
+
+ if(this->tracking)
+ {
+ send_head_target(pan_angle, tilt_angle);
+ }
+ watchdog_qr_lost.reset(ros::Duration(5));
+ //counter=0;
+ }else{
+ qr_detected=false;
+ //counter++;
+ }
+
+
+ //unlock previously blocked shared variables
+ //this->alg_.unlock();
+ this->qr_pose_mutex_exit();
+}
+
+void TrackChargeStationAlgNode::qr_pose_mutex_enter(void)
+{
+ pthread_mutex_lock(&this->qr_pose_mutex_);
+}
+
+void TrackChargeStationAlgNode::qr_pose_mutex_exit(void)
+{
+ pthread_mutex_unlock(&this->qr_pose_mutex_);
+}
+
+
+/* [service callbacks] */
+
+/* [action callbacks] */
+void TrackChargeStationAlgNode::head_follow_targetDone(const actionlib::SimpleClientGoalState& state, const humanoid_common_msgs::humanoid_follow_targetResultConstPtr& result)
+{
+ alg_.lock();
+ if( state == actionlib::SimpleClientGoalState::SUCCEEDED )
+ ROS_INFO("TrackChargeStationAlgNode::head_follow_targetDone: Goal Achieved!");
+ else
+ ROS_INFO("TrackChargeStationAlgNode::head_follow_targetDone: %s", state.toString().c_str());
+
+ //copy & work with requested result
+ alg_.unlock();
+}
+
+void TrackChargeStationAlgNode::head_follow_targetActive()
+{
+ alg_.lock();
+ //ROS_INFO("TrackChargeStationAlgNode::head_follow_targetActive: Goal just went active!");
+ alg_.unlock();
+}
+
+void TrackChargeStationAlgNode::head_follow_targetFeedback(const humanoid_common_msgs::humanoid_follow_targetFeedbackConstPtr& feedback)
+{
+ alg_.lock();
+ //ROS_INFO("TrackChargeStationAlgNode::head_follow_targetFeedback: Got Feedback!");
+
+ bool feedback_is_ok = true;
+
+ //analyze feedback
+ //my_var = feedback->var;
+
+ //if feedback is not what expected, cancel requested goal
+ if( !feedback_is_ok )
+ {
+ head_follow_target_client_.cancelGoal();
+ //ROS_INFO("TrackChargeStationAlgNode::head_follow_targetFeedback: Cancelling Action!");
+ }
+ alg_.unlock();
+}
+
+
+/* [action requests] */
+bool TrackChargeStationAlgNode::head_follow_targetMakeActionRequest()
+{
+ // IMPORTANT: Please note that all mutex used in the client callback functions
+ // must be unlocked before calling any of the client class functions from an
+ // other thread (MainNodeThread).
+ // this->alg_.unlock();
+ if(head_follow_target_client_.isServerConnected())
+ {
+ //ROS_DEBUG("TrackChargeStationAlgNode::head_follow_targetMakeActionRequest: Server is Available!");
+ //send a goal to the action server
+ //head_follow_target_goal_.data = my_desired_goal;
+ head_follow_target_client_.sendGoal(head_follow_target_goal_,
+ boost::bind(&TrackChargeStationAlgNode::head_follow_targetDone, this, _1, _2),
+ boost::bind(&TrackChargeStationAlgNode::head_follow_targetActive, this),
+ boost::bind(&TrackChargeStationAlgNode::head_follow_targetFeedback, this, _1));
+ // this->alg_.lock();
+ // ROS_DEBUG("TrackChargeStationAlgNode::MakeActionRequest: Goal Sent.");
+ return true;
+ }
+ else
+ {
+ // this->alg_.lock();
+ // ROS_DEBUG("TrackChargeStationAlgNode::head_follow_targetMakeActionRequest: HRI server is not connected");
+ return false;
+ }
+}
+
+
+void TrackChargeStationAlgNode::node_config_update(Config &config, uint32_t level)
+{
+ this->alg_.lock();
+
+ if(config.start) //Equivale a la funcion start_tracking
+ {
+ z_distance_target=config.z_target;
+ // x_distance_target=config.x_target;
+ //orientation_target=config.yaw_target;
+ z_min_tolerance=config.z_tolerance;
+ //x_min_error=config.x_error;
+ //rot_min_error=config.yaw_error;
+ new_goal=true;
+ config.start=false;
+ }
+ if(config.load)
+ {
+ z_distance.kp=config.z_kp;
+ z_distance.ki=config.z_ki;
+ z_distance.kd=config.z_kd;
+ z_distance.period=config.period;
+
+ config.load=false;
+ }
+ if(config.stop)
+ {
+ end=true;
+ config.stop=false;
+ }
+
+ this->config_=config;
+ this->alg_.unlock();
+}
+
+void TrackChargeStationAlgNode::addNodeDiagnostics(void)
+{
+}
+
+//////////////////////////
+
+
+
+////////////////////////
+//Start tracking state machine
+void TrackChargeStationAlgNode::start_tracking(double z_target, double x_target, double yaw_target)
+{
+ z_distance_target=z_target;
+ // x_distance_target=x_target;
+ // orientation_target=yaw_target;
+ new_goal=true; //Activar maquina de estados
+}
+
+bool TrackChargeStationAlgNode::set_servo_module_head(std::string module)
+{
+ this->set_servo_modules_srv_.request.names.resize(2);
+ this->set_servo_modules_srv_.request.modules.resize(2);
+ this->set_servo_modules_srv_.request.names[0]="j_pan";
+ this->set_servo_modules_srv_.request.modules[0]=module;
+ this->set_servo_modules_srv_.request.names[1]="j_tilt";
+ this->set_servo_modules_srv_.request.modules[1]=module;
+ if(this->set_servo_modules_client_.call(this->set_servo_modules_srv_))
+ return true;
+ else
+ {
+ ROS_DEBUG("Impossible to call set_servo_modules service");
+ return false;
+ }
+}
+
+bool TrackChargeStationAlgNode::set_servo_module(std::string module)
+{
+ unsigned int i;
+ this->set_servo_modules_srv_.request.names.resize(NUM_SERVOS);
+ this->set_servo_modules_srv_.request.modules.resize(NUM_SERVOS);
+ for(i=0;i<NUM_SERVOS;i++)
+ {
+ this->set_servo_modules_srv_.request.names[i]=servo_names[i];
+ this->set_servo_modules_srv_.request.modules[i]=module;
+ }
+ if(this->set_servo_modules_client_.call(this->set_servo_modules_srv_))
+ return true;
+ else
+ {
+ ROS_DEBUG("Impossible to call set_servo_modules service");
+ return false;
+ }
+
+}
+/*
+void TrackChargeStationAlgNode::pid(void) //distancia z
+{
+ z_distance.error = qr_position_z-z_distance_target;
+ //z_distance.error=z_distance_target-qr_position_z;
+ z_distance.derivative = (z_distance.error-z_distance.prev_error)/z_distance.period;
+ z_distance.integral = (z_distance.error*z_distance.period); //+=
+ //pid += z_distance.kp*z_distance.error + z_distance.ki*z_distance.integral + z_distance.kd*z_distance.derivative;
+ pid = z_distance.kp*z_distance.error;
+ z_distance.prev_error=z_distance.error;
+ ROS_INFO("ERROR: %f, PID: %f", z_distance.error, pid);
+ //ROS_INFO("Integral: %f, Derivada: %f", z_distance.integral, z_distance.derivative);
+ //ROS_INFO("qr X: %f, qr Y: %f, qr Z: %f", qr_position_x, qr_position_y, qr_position_z);
+
+ //dependiendo de pid --> modificar y_amplitude de cmd_vel
+ pid_out = pid/100;
+ ROS_INFO("PID 2: %f", pid_out);
+
+ if(pid_out>0.04)//maximo cmd_vel
+ return 0.04;
+ else if(pid_out<-0.04)
+ return -0.04;
+ else
+ return pid_out;
+}
+*/
+
+void TrackChargeStationAlgNode::pid2(TPID caca, double &culo) //distancia z
+{
+ this->new_time=ros::Time::now();
+ //this->time_period = (new_time-this->start_time);
+ time_period = (new_time-this->start_time).toSec();
+
+ z_distance.error = qr_position_z-z_distance_target;
+// if(z_distance.error<1)
+// {
+// return true;
+// }
+// else
+// {
+ z_distance.derivative = (z_distance.error-z_distance.prev_error)/this->time_period;
+ z_distance.integral = (z_distance.error*this->time_period); //+=
+ //pid += z_distance.kp*z_distance.error + z_distance.ki*z_distance.integral + z_distance.kd*z_distance.derivative;
+ pid = z_distance.kp*z_distance.error;
+ z_distance.prev_error=z_distance.error;
+ ROS_INFO("ERROR: %f, PID: %f", z_distance.error, pid);
+ //ROS_INFO("Integral: %f, Derivada: %f", z_distance.integral, z_distance.derivative);
+ //ROS_INFO("qr X: %f, qr Y: %f, qr Z: %f", qr_position_x, qr_position_y, qr_position_z);
+
+ //dependiendo de pid --> modificar y_amplitude de cmd_vel
+ pid_out = pid/100;
+ ROS_INFO("PID 2: %f", pid_out);
+ //command_vel=pid/100;
+
+ //return false;
+ //}
+ this->start_time=ros::Time::now();
+}
+
+//Head tracking
+void TrackChargeStationAlgNode::set_head_tracking_goal(double pan_target, double tilt_target)
+{
+ this->head_follow_target_goal_.target_pan=pan_target;
+ this->head_follow_target_goal_.target_tilt=tilt_target;
+ // this->head_follow_target_goal_.pan_range.resize(2);
+ /* this->head_follow_target_goal_.pan_range[0]=this->config_.max_pan;
+ this->head_follow_target_goal_.pan_range[1]=this->config_.min_pan;
+ this->head_follow_target_goal_.tilt_range.resize(2);
+ this->head_follow_target_goal_.tilt_range[0]=this->config_.max_tilt;
+ this->head_follow_target_goal_.tilt_range[1]=this->config_.min_tilt;
+ */
+ this->head_follow_target_goal_.pan_range[0]=1.5;
+ this->head_follow_target_goal_.pan_range[1]=-1.5;
+ //this->head_follow_target_goal_.tilt_range.resize(2);
+ this->head_follow_target_goal_.tilt_range[0]=1;
+ this->head_follow_target_goal_.tilt_range[1]=-1;
+}
+
+void TrackChargeStationAlgNode::send_head_target(double pan, double tilt)
+{
+ //this->head_target_JointTrajectoryPoint_msg_.positions.resize(2);
+ this->head_target_JointTrajectoryPoint_msg_.positions[0]=pan;
+ this->head_target_JointTrajectoryPoint_msg_.positions[1]=tilt;
+ this->head_target_publisher_.publish(this->head_target_JointTrajectoryPoint_msg_);
+}
+
+//Walk
+bool TrackChargeStationAlgNode::set_walk_params(void)
+{
+ set_walk_params_srv_.request.params.Y_SWAP_AMPLITUDE = config_.Y_SWAP_AMPLITUDE;
+ set_walk_params_srv_.request.params.Z_SWAP_AMPLITUDE = config_.Z_SWAP_AMPLITUDE;
+ set_walk_params_srv_.request.params.ARM_SWING_GAIN = config_.ARM_SWING_GAIN;
+ set_walk_params_srv_.request.params.PELVIS_OFFSET = config_.PELVIS_OFFSET;
+ set_walk_params_srv_.request.params.HIP_PITCH_OFFSET = config_.HIP_PITCH_OFFSET;
+ set_walk_params_srv_.request.params.X_OFFSET = config_.X_OFFSET;
+ set_walk_params_srv_.request.params.Y_OFFSET = config_.Y_OFFSET;
+ set_walk_params_srv_.request.params.Z_OFFSET = config_.Z_OFFSET;
+ set_walk_params_srv_.request.params.A_OFFSET = config_.A_OFFSET;
+ set_walk_params_srv_.request.params.P_OFFSET = config_.P_OFFSET;
+ set_walk_params_srv_.request.params.R_OFFSET = config_.R_OFFSET;
+ set_walk_params_srv_.request.params.PERIOD_TIME = config_.PERIOD_TIME;
+ set_walk_params_srv_.request.params.DSP_RATIO = config_.DSP_RATIO;
+ set_walk_params_srv_.request.params.STEP_FB_RATIO = config_.STEP_FB_RATIO;
+ set_walk_params_srv_.request.params.FOOT_HEIGHT = config_.FOOT_HEIGHT;
+ set_walk_params_srv_.request.params.MAX_VEL = config_.MAX_VEL;
+ set_walk_params_srv_.request.params.MAX_ROT_VEL = config_.MAX_ROT_VEL;
+ ROS_INFO("SmQrSearchAlgNode:: Sending New Request!");
+ if(set_walk_params_client_.call(set_walk_params_srv_))
+ {
+ if(set_walk_params_srv_.response.ret)
+ ROS_INFO("SmQrSearchAlgNode:: Parameters changed successfully");
+ else
+ ROS_INFO("SmQrSearchAlgNode:: Impossible to change parameters");
+ }
+ else
+ ROS_INFO("SmQrSearchAlgNode:: Failed to Call Server on topic set_walk_params ");
+
+}
+
+void TrackChargeStationAlgNode::send_cmd_vel(double x_amplitude, double y_amplitude, double a_amplitude)
+{
+ /* this->cmd_vel_Twist_msg_.linear.x=x_amplitude/period_time;
+ this->cmd_vel_Twist_msg_.linear.y=y_amplitude/period_time;
+ this->cmd_vel_Twist_msg_.linear.z=0;
+ this->cmd_vel_Twist_msg_.angular.x=0;
+ this->cmd_vel_Twist_msg_.angular.y=0;
+ this->cmd_vel_Twist_msg_.angular.z=a_amplitude/period_time;
+*/
+ this->cmd_vel_Twist_msg_.linear.x=x_amplitude; //longitudinal. de -0.04 a 0.04 en metros
+ this->cmd_vel_Twist_msg_.linear.y=y_amplitude; //Transversal
+ this->cmd_vel_Twist_msg_.linear.z=0;
+ this->cmd_vel_Twist_msg_.angular.x=0;
+ this->cmd_vel_Twist_msg_.angular.y=0;
+ this->cmd_vel_Twist_msg_.angular.z=a_amplitude; //de -0.5 a 0.5 en radianes
+
+ this->cmd_vel_publisher_.publish(this->cmd_vel_Twist_msg_);
+}
+
+void TrackChargeStationAlgNode::stop_walking(void)
+{
+ this->cmd_vel_Twist_msg_.linear.x=0;
+ this->cmd_vel_Twist_msg_.linear.y=0;
+ this->cmd_vel_Twist_msg_.linear.z=0;
+ this->cmd_vel_Twist_msg_.angular.x=0;
+ this->cmd_vel_Twist_msg_.angular.y=0;
+ this->cmd_vel_Twist_msg_.angular.z=0;
+
+ this->cmd_vel_publisher_.publish(this->cmd_vel_Twist_msg_);
+}
+
+
+/*
+void pid(TPID algo)
+{
+ algo.error=z_distance_target-qr_position_z;
+ integral+=(z_distance.error*period)/1000; //>>16
+ derivada=((z_distance.error-z_distance.prev_error)*1000)period //<<16
+ posicion+= (z_distance.kp*error) + (ki*integral) + (kd*derivada);
+ prev_error=error;
+
+ if(z_distance.integral>i_clamp)
+ z_distance-integral=i_clamp;
+ else if(z_distance.integral<i_clamp)
+ z_distance.integral-=i_clamp;
+
+}*/
+
+/* main function */
+int main(int argc,char *argv[])
+{
+ return algorithm_base::main<TrackChargeStationAlgNode>(argc, argv, "track_charge_station_alg_node");
+}