diff --git a/automatic_charge/cfg/AutomaticCharge.cfg b/automatic_charge/cfg/AutomaticCharge.cfg
index 833818d8d80e93b21d88b66332eaaca8b8347024..cd389a5d793c22a412e25a3d6dd3ab70279f9042 100755
--- a/automatic_charge/cfg/AutomaticCharge.cfg
+++ b/automatic_charge/cfg/AutomaticCharge.cfg
@@ -60,12 +60,12 @@ gen.add("yaw_step", double_t, 0, "
gen.add("angle_rotation", double_t, 0, "Angle of rotation", 3.1415, 0.78, 3.1415)
gen.add("error", double_t, 0, "Min error in rotation", 0.01, 0.01, 0.1)
#tracking module
-gen.add("z_target", double_t, 0, "Z target distance to charge station", 0.3, 0.0, 2.0)
+gen.add("z_target", double_t, 0, "Z target distance to charge station", 0.22, 0.0, 2.0)
gen.add("x_target", double_t, 0, "X target distance to charge station", 0.0, 0.0, 2.0)
gen.add("yaw_target", double_t, 0, "Orientation target to charge station", -0.4, -0.5, 0.5)
-gen.add("z_tolerance", double_t, 0, "Tolerance in distance z", 0.1, 0.0, 5)
-gen.add("x_tolerance", double_t, 0, "Tolerance in distance z", 0.05, 0.0, 5)
-gen.add("yaw_tolerance", double_t, 0, "Tolerance in yaw", 0.1, -10, 10)
+gen.add("z_tolerance", double_t, 0, "Tolerance in distance z", 0.05, 0.0, 5)
+gen.add("x_tolerance", double_t, 0, "Tolerance in distance z", 0.04, 0.0, 5)
+gen.add("yaw_tolerance", double_t, 0, "Tolerance in yaw", 0.08, -10, 10)
gen.add("config_pid", bool_t, 0, "Enable/Disable smart charger module", False)
gen.add("z_kp", double_t, 0, "Desired kp for z axis", 0.05, -10, 10)
gen.add("x_kp", double_t, 0, "Desired kp fpr x axis", 0.5, -10, 10)
diff --git a/automatic_charge/include/automatic_charge_alg_node.h b/automatic_charge/include/automatic_charge_alg_node.h
index 88774bb9a7e668a20b4cedc3461eafb7e669a767..9b020ac891e59f73b2b68bd4e2b2542cbffc40fa 100644
--- a/automatic_charge/include/automatic_charge_alg_node.h
+++ b/automatic_charge/include/automatic_charge_alg_node.h
@@ -47,6 +47,7 @@
// [service client headers]
#include <humanoid_common_msgs/get_smart_charger_config.h>
#include <humanoid_common_msgs/set_smart_charger_config.h>
+#include <humanoid_common_msgs/set_qr_size.h>
// [action server client headers]
@@ -110,6 +111,9 @@ class AutomaticChargeAlgNode : public algorithm_base::IriBaseAlgorithm<Automatic
ros::ServiceClient set_smart_charger_config_client_;
humanoid_common_msgs::set_smart_charger_config set_smart_charger_config_srv_;
+ ros::ServiceClient set_qr_size_client_;
+ humanoid_common_msgs::set_qr_size set_qr_size_srv_;
+
// [action server attributes]
// [action client attributes]
@@ -130,7 +134,8 @@ class AutomaticChargeAlgNode : public algorithm_base::IriBaseAlgorithm<Automatic
humanoid_common_msgs::smart_charger_data charger_data; //saves data received from smart_charger_data_callback
//qr
- std::string qr_id;
+ bool qr_detected;
+ std::vector<std::string> qr_id;
humanoid_common_msgs::tag_pose_array::ConstPtr qr_data;
CROSWatchdog watchdog_qr_lost;
@@ -152,7 +157,7 @@ class AutomaticChargeAlgNode : public algorithm_base::IriBaseAlgorithm<Automatic
// void process_data(void);
- bool active, active2;
+ bool active, active2, active3, find_int_point, no_int_sol, send, second_try;
/**
* \brief config variable
@@ -220,6 +225,7 @@ class AutomaticChargeAlgNode : public algorithm_base::IriBaseAlgorithm<Automatic
// [test functions]
void config_smart_charger(bool enable, bool disable, double period, double limit_current);
+ void qr_size(double qr_x, double qr_y);
};
#endif
diff --git a/automatic_charge/launch/automatic_charge_sim.launch b/automatic_charge/launch/automatic_charge_sim.launch
index 80e36742375239777cfb7bc111b8091280e59b09..cdb65d69e97d806715bd47efd991d9b5046e3ad9 100644
--- a/automatic_charge/launch/automatic_charge_sim.launch
+++ b/automatic_charge/launch/automatic_charge_sim.launch
@@ -20,17 +20,23 @@
<param name="search_module/joint_trajectory/enable_timeout" value="false"/>
<param name="search_module/joint_trajectory/rate_hz" value="10.0"/>
<param name="joint_trajectory/rate_hz" value="5.0"/>
- <param name="search_module/walk/rate_hz" value="5.0"/>
+ <param name="search_module/walk/rate_hz" value="30.0"/>
<param name="walk/cmd_vel_rate_hz" value="50.0"/>
- <param name="tracking_module/rate_hz" value="6"/>
- <param name="tracking_module/head_tracking/rate_hz" value="10.0"/>
+ <param name="tracking_module/rate_hz" value="4"/>
+ <param name="tracking_module/head_tracking/rate_hz" value="40.0"/>
+ <param name="tracking_module/walk/rate_hz" value="30.0"/>
<param name="search_module/rate_hz" value="6"/>
+ <param name="action_module/rate_hz" value="10"/>
<param name="action_module/enable_timeout" value="false"/>
<param name="joints_cart_module/enable_timeout" value="false"/>
<param name="joints_cart_module/get_ik_max_retries" value="3"/>
+ <param name="joints_cart_module/rate_hz" value="10"/>
+ <!--<param name="left_gripper/enable_timeout" value="false"/>-->
<remap from="/darwin/automatic_charge/qr_pose"
to="/qr_detector/qr_pose"/>
+ <remap from="/darwin/automatic_charge/set_qr_size"
+ to="/qr_detector/set_qr_size"/>
<!-- Smart charger -->
<remap from="/darwin/automatic_charge/smart_charger_data"
to="/darwin/robot/smart_charger_data"/>
@@ -160,8 +166,8 @@
name="qr_detector"
type="qr_detector"
output="screen">
- <param name="qr_x" value="0.04"/>
- <param name="qr_y" value="0.04"/>
+ <param name="qr_x" value="0.1"/>
+ <param name="qr_y" value="0.1"/>
<param name="camera_frame" value="/darwin/darwin_cam_optical_link"/>d
<remap from="~/camera/image_raw" to="/darwin/camera/image_raw"/>
<remap from="~/camera/camera_info" to="/darwin/camera/camera_info"/>
diff --git a/automatic_charge/src/automatic_charge_alg_node.cpp b/automatic_charge/src/automatic_charge_alg_node.cpp
index 2d92091516958a109d6be2bf0e34207f1c6a8f01..a8b9a26900575b06613536ff201874955af2a514 100644
--- a/automatic_charge/src/automatic_charge_alg_node.cpp
+++ b/automatic_charge/src/automatic_charge_alg_node.cpp
@@ -65,9 +65,11 @@ AutomaticChargeAlgNode::AutomaticChargeAlgNode(void) :
// [init services]
// [init clients]
- get_smart_charger_config_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::get_smart_charger_config>("get_smart_charger_config");
+ this->get_smart_charger_config_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::get_smart_charger_config>("get_smart_charger_config");
- set_smart_charger_config_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_smart_charger_config>("set_smart_charger_config");
+ this->set_smart_charger_config_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_smart_charger_config>("set_smart_charger_config");
+
+ this->set_qr_size_client_ = this->public_node_handle_.serviceClient<humanoid_common_msgs::set_qr_size>("set_qr_size");
// [init action servers]
@@ -86,8 +88,19 @@ AutomaticChargeAlgNode::AutomaticChargeAlgNode(void) :
this->charged=false;
this->discharged=false;
- this->active=false;
+ this->active=true; //envia primer punto cart
this->active2=false;
+ this->active3=false;
+ send=false;
+ find_int_point=false;
+ no_int_sol=false;
+ second_try=false;
+
+ this->qr_detected=false;
+ this->qr_id.resize(3);
+ this->qr_id[0]="W1";
+ this->qr_id[1]="S1";
+ this->qr_id[2]="N1";
}
AutomaticChargeAlgNode::~AutomaticChargeAlgNode(void)
@@ -112,6 +125,10 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
// disable smart charger
this->config_smart_charger(false, true, DEFAULT_PERIOD, DEFAULT_CURRENT);
// stop search
+ this->qr_id.clear();
+ this->qr_id.resize(2);
+ this->qr_id[0]="W1";
+ this->qr_id[1]="N1";
this->search_module.stop_search();
// stop tracking
this->tracking_module.stop_tracking();
@@ -122,6 +139,10 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
this->state=IDLE;
this->status=CANCELED;
this->stop=false;
+ //joints cart
+ find_int_point=false;
+ no_int_sol=false;
+ active=true; //para enviar el primer punto
}
else
{
@@ -130,10 +151,10 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
case IDLE: ROS_INFO("AutomaticChargeAlgNode: state IDLE");
if(this->start)
{
- this->action_module.execute(15);
+ //this->action_module.execute(15);
//
battery_status=CONNECTED;
- this->state=SIT;
+ this->state=SOC;
//
// this->state=SOC;
this->status=RUNNING;
@@ -149,8 +170,6 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
if(battery_status==DISCONNECTED)
{
ROS_INFO("AutomaticChargeAlgNode: Batteries disconnected!");
- // this->state=SEARCH;
- // this->search_module.start_search("W1", config_.angle_rotation, config_.error);
this->state=IDLE;
}
if(battery_status==CONNECTED)
@@ -159,14 +178,13 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
if(this->discharged)
{
ROS_INFO("AutomaticChargeAlgNode: Batteries discharged");
- // this->state=SEARCH;
- // this->search_module.start_search("W1", config_.angle_rotation, config_.error);
- this->tracking_module.start_tracking("W1", config_.z_target, config_.x_target, config_.yaw_target, config_.z_tolerance, config_.x_tolerance, config_.yaw_tolerance);
- this->state=TRACK;
+ this->qr_size(0.1,0.1); //Change QR size to detect large QR
+ this->search_module.start_search(qr_id, config_.angle_rotation, config_.error);
+ this->state=SEARCH;
}
- }
else
this->state=SOC;
+ }
if(battery_status==CHARGING)
{
ROS_INFO("AutomaticChargeAlgNode: Batteries connected and charging");
@@ -198,7 +216,7 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
switch(this->search_module.get_status())
{
case SEARCH_MODULE_SUCCESS: ROS_INFO("AutomaticChargeAlgNode: Search completed successfully!");
- this->tracking_module.start_tracking("W1", config_.z_target, config_.x_target, config_.yaw_target, config_.z_tolerance, config_.x_tolerance, config_.yaw_tolerance);
+ this->tracking_module.start_tracking(qr_id, config_.z_target, config_.x_target, config_.yaw_target, config_.z_tolerance, config_.x_tolerance, config_.yaw_tolerance);
this->state=TRACK;
break;
case SEARCH_MODULE_CANCELED: ROS_INFO("AutomaticChargeAlgNode: Search canceled");
@@ -246,9 +264,8 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
this->state=IDLE;
break;
case TRACK_MODULE_LOST: ROS_INFO("AutomaticChargeAlgNode: QR lost!");
- this->search_module.start_search("W1", config_.angle_rotation, config_.error);
+ this->search_module.start_search(qr_id, config_.angle_rotation, config_.error);
this->state=SEARCH;
- this->status=CANCELED;
break;
case TRACK_MODULE_HEAD_ERROR: ROS_INFO("AutomaticChargeAlgNode: Track failed due to error in head_tracking submodule");
this->state=END;
@@ -274,17 +291,20 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
this->state=SIT;
else
{
- ROS_INFO("AutomaticChargeAlgNode: End of moving head");
+ //ROS_INFO("AutomaticChargeAlgNode: End of moving head");
if(this->action_module.get_status()==MOTION_MODULE_SUCCESS)
{
- ROS_INFO("AutomaticChargeAlgNode: Moved head completed successfully");
- //Mover cabeza (para ver los QR pequeños) + abrir grippers
- this->search_module.start_search("W1R", config_.angle_rotation, config_.error, 0.8);
- //mover brazo a posicion conocida
+ //ROS_INFO("AutomaticChargeAlgNode: Moved head completed successfully");
+ this->qr_size(0.035,0.035); //Change QR size for detection of small QR
+ this->qr_id.resize(4);
+ this->qr_id[0]="W1R";
+ this->qr_id[1]="W1L";
+ this->qr_id[2]="N1R";
+ this->qr_id[3]="N1L";
+ this->search_module.start_search(qr_id, config_.angle_rotation, config_.error, 0.8);
// this->joints_module.add_goal(4,-1.5,1,0.3); //move tilt
// this->joints_module.execute_goal();
- //Abrir gripper
- this->left_gripper.open();
+ this->left_gripper.open(); //Open gripper
this->state=MOVE_HEAD;
}
else
@@ -305,47 +325,58 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
this->search_module.start_search("W1", config_.angle_rotation, config_.error);
this->state=MOVE_HEAD;
*/
- case MOVE_HEAD: ROS_INFO("AutomaticChargeAlgNode: state MOVE_HEAD");
- if(/*!this->joints_module.is_finished()*/ !this->search_module.is_finished()/* && !this->left_gripper.is_finished()*/)
- this->state=MOVE_HEAD;
- else
+
+ case MOVE_HEAD: ROS_INFO("AutomaticChargeAlgNode: state MOVE_HEAD");
+ if(qr_detected/* || this->search_module.is_finished() *//*!this->joints_module.is_finished() && !this->left_gripper.is_finished()*/)
{
+ find_int_point=true;
+ //active=true; //envia primer punto (2)
+
//Llamar a la funcion que calculara el punto al que ir
- ROS_INFO("AutomaticChargeAlgNode: SEND JOINTS CARTESIAN");
- this->gripper_position.header.stamp=ros::Time::now();
- this->gripper_position.header.frame_id="/darwin/base_arm_l";
+
// this->gripper_position.point.x=-0.0616414;
// this->gripper_position.point.y=-0.0426087;
// this->gripper_position.point.z=0.0220413;
// this->gripper_position.point.z=0.000000;
- this->state=MOVE_HEAD;
+ /* this->state=MOVE_HEAD;
this->active=true;
if(this->active2)
{
+ ROS_INFO("AutomaticChargeAlgNode: SEND JOINTS CARTESIAN");
+ this->gripper_position.header.stamp=ros::Time::now();
+ this->gripper_position.header.frame_id="/darwin/base_arm_l";
this->joints_cart_module.set_default_speed(1.5);
this->joints_cart_module.go_to_position(gripper_position);
this->state=MOVE_ARM;
- }
+ }else if(no_int_sol)
+ this->state=END;
+ */
+ if(send) //se ha enviado el punto
+ state=MOVE_ARM;
+ else
+ state=MOVE_HEAD;
}
break;
-
+
case MOVE_ARM: ROS_INFO("AutomaticChargeAlgNode: state MOVE_ARM");
+ send=false;
if(this->joints_cart_module.is_finished())
{
+
//if(joints_cart_module.get_status()==JOINTS_CART_MODULE_SUCCESS)
switch(this->joints_cart_module.get_status())
{
case JOINTS_CART_MODULE_SUCCESS: ROS_INFO("AutomaticChargeAlgNode: Moved arm successfully");
- //Cerrar gripper
+ //Close gripper
this->left_gripper.close();
this->state=CONNECT;
break;
case JOINTS_CART_MOUDLE_GET_IK_FAIL: ROS_INFO("AutomaticChargeAlgNode: Failed to move arm");
- //Volver a posicionarse
this->state=MOVE_HEAD;
+ active2=true; //send second point
break;
}
}
@@ -364,12 +395,10 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
if(this->battery_status==CHARGING)
{
ROS_INFO("Batteries connected!");
- //Leer smart charger hasta que este cargado
- this->state=SOC;
+ this->state=SOC; //Read smart charger data until its charged
}
else if(this->battery_status==CONNECTED)
- //Volver a intentar conectarse
- //this->state=MOVE_ARM;
+ //Try to connect again
this->state=MOVE_ARM;
}
break;
@@ -382,7 +411,6 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
break;
case END: ROS_INFO("AutomaticChargeAlgNode: state END"); //stop all modules
- //posibles errores, estado etc
// disable smart charger
this->config_smart_charger(false, true, DEFAULT_PERIOD, DEFAULT_CURRENT);
// stop search
@@ -468,6 +496,7 @@ void TrackChargeStationAlgNode::tf_cam_shoulder(void)
/* [subscriber callbacks] */
void AutomaticChargeAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_pose_array::ConstPtr& msg)
{
+ //ROS_WARN("QR DISTANCE: %f, ORIENTATION: %f", msg->tags[0].position.z, tf::getYaw(msg->tags[0].orientation));
//ROS_INFO("CSearchModule::qr_pose_callback: New Message Received");
unsigned int i;
double z;
@@ -477,10 +506,11 @@ void AutomaticChargeAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_po
if(msg->tags.size()>0)
{
- if(msg->tags[0].tag_id=="W1R")
+ this->qr_detected=true;
+ if(msg->tags[0].tag_id=="W1R" || msg->tags[0].tag_id=="W1L" || msg->tags[0].tag_id=="N1L" || msg->tags[0].tag_id=="N1L")
+ {
+ if(find_int_point)
{
- // ROS_WARN("out x: %f, out y: %f, out z: %f",msg->tags[0].position.x,msg->tags[0].position.y,msg->tags[0].position.z );
-
std::string source_frame="darwin/left_shoulder";
std::string target_frame="station_connector_link";
@@ -536,8 +566,8 @@ void AutomaticChargeAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_po
ROS_INFO("Y1: %f, X1: %f",y1, x1);
ROS_INFO("Y2: %f, X2: %f",y2,x2);
- }else
- ROS_ERROR("AutomaticChargeAlgNode: Not found solution");
+ no_int_sol=true;
+
///////////
@@ -558,6 +588,20 @@ void AutomaticChargeAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_po
tf_listener.transformPose(target_frame, in, out);
ROS_WARN("!!!! out arm x2: %f, out arm y2: %f, out arm z2: %f",out.pose.position.x,out.pose.position.y,out.pose.position.z );
+ //Send point 2 to joints cart
+ if(active)
+ {
+ ROS_WARN("AutomaticChargeAlgNode: SEND JOINTS CARTESIAN x2");
+ this->gripper_position.header.stamp=ros::Time::now();
+ this->gripper_position.header.frame_id="/darwin/base_arm_l";
+ this->gripper_position.point.y=out.pose.position.y; //respecto el disco
+ this->gripper_position.point.x=out.pose.position.x;
+ this->gripper_position.point.z=out.pose.position.z;
+ this->joints_cart_module.set_default_speed(1.5);
+ this->joints_cart_module.go_to_position(gripper_position);
+ active=false;
+ send=true;
+ }
in.header.frame_id = source_frame;
in.header.stamp = target_time;
in.pose.position.x = x1;
@@ -572,14 +616,27 @@ void AutomaticChargeAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_po
}
- if(this->active)
+ if(this->active2)
{
+ this->gripper_position.header.stamp=ros::Time::now();
+ this->gripper_position.header.frame_id="/darwin/base_arm_l";
this->gripper_position.point.y=out.pose.position.y; //respecto el disco
this->gripper_position.point.x=out.pose.position.x;
this->gripper_position.point.z=out.pose.position.z;
- ROS_WARN("!!!! out arm x1: %f, out arm y1: %f, out arm z1: %f",out.pose.position.x,out.pose.position.y,out.pose.position.z );
- this->active2=true;
+ this->joints_cart_module.set_default_speed(1.5);
+ this->joints_cart_module.go_to_position(gripper_position);
+ ROS_WARN("SEND JOINT CARTESIAN x1");
+ active2=false;
+ send=true;
+ second_try=true;
+ //this->active2=true;
}
+
+
+ }else{
+ ROS_ERROR("AutomaticChargeAlgNode: Not found solution");
+ no_int_sol=true;
+ }
//////////
@@ -590,12 +647,12 @@ void AutomaticChargeAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_po
}
-
+ }
// qr_detected=true;
this->watchdog_qr_lost.reset(ros::Duration(1));
}else{
- ROS_WARN("QR NOT DETECTED");
+ ROS_INFO("AutomaticChargeAlgNode:: QR NOT Detected");
}
this->qr_pose_access.exit();
@@ -879,6 +936,18 @@ void AutomaticChargeAlgNode::process_data(void)
}
*/
+void AutomaticChargeAlgNode::qr_size(double qr_x, double qr_y)
+{
+ set_qr_size_srv_.request.qr_x=qr_x;
+ set_qr_size_srv_.request.qr_y=qr_y;
+ if(set_qr_size_client_.call(set_qr_size_srv_))
+ {
+ //if(set_qr_size_srv_.response.success)
+ ROS_INFO_STREAM("AutomaticChargeAlgNode:: Service QR size called");
+ }
+ else
+ ROS_INFO_STREAM("AutomaticChargeAlgNode:: Failed to call server set_qr_size");
+}
void AutomaticChargeAlgNode::config_smart_charger(bool enable, bool disable, double period, double limit_current)
{