diff --git a/automatic_charge/include/automatic_charge_alg_node.h b/automatic_charge/include/automatic_charge_alg_node.h
index 0949867322cce6dffe9321e399879fd5537698a8..9c036aa1709c858b84153f78e614e2580bd0db3f 100644
--- a/automatic_charge/include/automatic_charge_alg_node.h
+++ b/automatic_charge/include/automatic_charge_alg_node.h
@@ -32,6 +32,7 @@
#include <humanoid_modules/search_module.h>
#include <humanoid_modules/tracking_module.h>
#include <humanoid_modules/action_module.h>
+#include <humanoid_modules/joints_module.h>
#include <humanoid_modules/joints_cart_module.h>
// [publisher subscriber headers]
@@ -129,6 +130,7 @@ class AutomaticChargeAlgNode : public algorithm_base::IriBaseAlgorithm<Automatic
CSearchModule search_module;
CTrackingModule tracking_module;
CActionModule action_module;
+ CJointsModule joints_module;
CJointsCartModule joints_cart_module;
diff --git a/automatic_charge/launch/automatic_charge_sim.launch b/automatic_charge/launch/automatic_charge_sim.launch
index 1c2c1ef75b405e443b048fd77c54d6a94ac5aefe..2d6e8a95eb06490029688c18bf2e17cb7ff88222 100644
--- a/automatic_charge/launch/automatic_charge_sim.launch
+++ b/automatic_charge/launch/automatic_charge_sim.launch
@@ -25,6 +25,10 @@
<param name="tracking_module/rate_hz" value="6"/>
<param name="tracking_module/head_tracking/rate_hz" value="10.0"/>
<param name="search_module/rate_hz" value="6"/>
+ <param name="action_module/enable_timeout" value="false"/>
+
+ <remap from="/darwin/automatic_charge/qr_pose"
+ to="/qr_detector/qr_pose"/>
<!-- Smart charger -->
<remap from="/darwin/automatic_charge/smart_charger_data"
to="/darwin/robot/smart_charger_data"/>
@@ -37,6 +41,13 @@
to="/darwin/robot/motion_action"/>
<remap from="/darwin/automatic_charge/action_module/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
+<!-- joints module -->
+ <remap from="/darwin/automatic_charge/joints_module/joint_trajectory_action"
+ to="/darwin/robot/joint_trajectory"/>
+ <remap from="/darwin/automatic_charge/joints_module/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/automatic_charge/joints_module/joint_state"
+ to="/darwin/robot/joint_state"/>
<!-- Search module -->
<remap from="/darwin/automatic_charge/search_module/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
diff --git a/automatic_charge/src/automatic_charge_alg_node.cpp b/automatic_charge/src/automatic_charge_alg_node.cpp
index 1225877d38a5e9e84ad1ab2dce6071db93bead22..93ee93c6be389ea3bb58588684e9b158d35275a9 100644
--- a/automatic_charge/src/automatic_charge_alg_node.cpp
+++ b/automatic_charge/src/automatic_charge_alg_node.cpp
@@ -7,11 +7,12 @@
*
* TO DO:
* -ID del QR a seguir (para que no coja uno pequeño en el track) -OK
- * -Añadir action sit
+ * -Añadir action sit -OK
* -añadir state of fall al tracking (o algun sitio)
* -poner un estado wait para que no salga que la maquina de estados de search o tracking haya acabado hasta que no se hayan parado los submodulos
* -introducir un vector de strings con las id de los 4 qr, escojer uno y seguirlo
* -propagar errores head_tracking y walk al modulo tracking
+ * -Esperar a que los submodulos hayan terminado para poner el modulo a finished
*
* -Tracking: funcion is_running en head_tracking y walk - comprobar si hay error en los submodulos - poner funcion con timer que lo compruebe? poner un estado?
*
@@ -26,6 +27,8 @@
* -si llega en diagonal al qr --> yaw=40, z=50cm pero si llega frontal (yaw=0, Z=30) y ponemos esos minimos quedara demasiado lejos
*
* -Arco del brazo?
+ * -Probar ejemplos driver bajo nivel darwin_arm_kin.cpp y cojer un punto para poner en la maquina de estados
+ * -Tendra que ir antes a una posición lateral? para asegurar que no choque con la etsacion
*
* NOTAS:
* ID QR: N1,N1L, N1R / W1, W1L, W1R / S1 y E1
@@ -43,6 +46,7 @@ AutomaticChargeAlgNode::AutomaticChargeAlgNode(void) :
search_module("search_module"),
tracking_module("tracking_module"),
action_module("action_module"),
+ joints_module("joints_module"),
joints_cart_module("joints_cart_module")
{
//init class attributes if necessary
@@ -173,9 +177,15 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
break;
case SEARCH_MODULE_ERROR_JOINTS: ROS_INFO("AutomaticChargeAlgNode: Search failed due to error in joints submodule");
this->state=IDLE;
+ this->status=SEARCH_FAIL;
break;
case SEARCH_MODULE_ERROR_WALK: ROS_INFO("AutomaticChargeAlgNode: Search failed due to error in walk submodule");
- this->state=IDLE;
+ if(this->search_module.get_walk_module_status()==WALK_MODULE_ROBOT_FELL)
+ {
+ //levantarse
+ }
+ this->state=END;
+ this->status=SEARCH_FAIL;
break;
}
}
@@ -206,8 +216,15 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
this->status=TRACKING_FAIL;
break;
case TRACK_MODULE_WALK_ERROR: ROS_INFO("AutomaticChargeAlgNode: Track failed due to error in walk submodule");
- this->state=END;
- this->status=TRACKING_FAIL;
+ if(this->search_module.get_walk_module_status()==WALK_MODULE_ROBOT_FELL)
+ {
+ //levantarse
+ }
+ else
+ {
+ this->state=END;
+ this->status=TRACKING_FAIL;
+ }
break;
}
}
@@ -222,6 +239,8 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
if(this->action_module.get_status()==MOTION_MODULE_SUCCESS)
{
ROS_INFO("AutomaticChargeAlgNode: Action completed successfully");
+ this->joints_module.add_goal(20,0.8,1,0.3); //move tilt
+ this->joints_module.execute_goal();
this->state=END;
}
else
@@ -234,8 +253,10 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
break;
- case CONNECT: ROS_INFO("AutomaticChargeAlgNode. state CONNECT");
- //this->joints_cart_module.go_to_position();
+ case CONNECT: ROS_INFO("AutomaticChargeAlgNode: state CONNECT");
+ this->gripper_position.point.y=0.05;
+ this->gripper_position.point.z=0.5;
+ this->joints_cart_module.go_to_position(gripper_position);
break;
@@ -268,8 +289,24 @@ void AutomaticChargeAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_po
if(msg->tags.size()>0)
{
+ for(i=0; i<msg->tags.size(); i++)
+ {
+ if(msg->tags[i].tag_id=="W1L")
+ {
+ ROS_DEBUG("QR X L: %f cm", (msg->tags[i].position.x));
+ ROS_WARN("QR Y L: %f cm", (msg->tags[i].position.y));
+ ROS_WARN("QR Z L: %f cm", (msg->tags[i].position.z));
+ }
+ if(msg->tags[i].tag_id=="W1R")
+ {
+ ROS_DEBUG("QR X R: %f cm", (msg->tags[i].position.x));
+ ROS_WARN("QR Y R: %f cm", (msg->tags[i].position.y));
+ ROS_WARN("QR Z R: %f cm", (msg->tags[i].position.z));
+ }
+ }
//qr_detected=msg->tags.size();
- /* qr_data=*msg;
+
+ /* qr_data=*msg;
for(i=0; i<msg->tags.size(); i++)
{