Skip to content
Snippets Groups Projects
Commit a1d65d0e authored by Irene Garcia Camacho's avatar Irene Garcia Camacho
Browse files

Last complete version of the automatic charge system 

Version presented on the final degree project
parent 90186c8d
No related branches found
No related tags found
1 merge request!4Filtered localization
Hide whitespace changes
Inline Side-by-side
automatic_charge/cfg/AutomaticCharge.cfg
+ 4
− 4
View file @ a1d65d0e
...@@ -60,12 +60,12 @@ gen.add("yaw_step", double_t, 0, " ...@@ -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("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) gen.add("error", double_t, 0, "Min error in rotation", 0.01, 0.01, 0.1)
#tracking module #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("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("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("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.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.1, -10, 10) 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("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("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) gen.add("x_kp", double_t, 0, "Desired kp fpr x axis", 0.5, -10, 10)
......
automatic_charge/include/automatic_charge_alg_node.h
+ 8
− 2
View file @ a1d65d0e
...@@ -47,6 +47,7 @@ ...@@ -47,6 +47,7 @@
// [service client headers] // [service client headers]
#include <humanoid_common_msgs/get_smart_charger_config.h> #include <humanoid_common_msgs/get_smart_charger_config.h>
#include <humanoid_common_msgs/set_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] // [action server client headers]
...@@ -110,6 +111,9 @@ class AutomaticChargeAlgNode : public algorithm_base::IriBaseAlgorithm<Automatic ...@@ -110,6 +111,9 @@ class AutomaticChargeAlgNode : public algorithm_base::IriBaseAlgorithm<Automatic
ros::ServiceClient set_smart_charger_config_client_; ros::ServiceClient set_smart_charger_config_client_;
humanoid_common_msgs::set_smart_charger_config set_smart_charger_config_srv_; 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 server attributes]
// [action client attributes] // [action client attributes]
...@@ -130,7 +134,8 @@ class AutomaticChargeAlgNode : public algorithm_base::IriBaseAlgorithm<Automatic ...@@ -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 humanoid_common_msgs::smart_charger_data charger_data; //saves data received from smart_charger_data_callback
//qr //qr
std::string qr_id; bool qr_detected;
std::vector<std::string> qr_id;
humanoid_common_msgs::tag_pose_array::ConstPtr qr_data; humanoid_common_msgs::tag_pose_array::ConstPtr qr_data;
CROSWatchdog watchdog_qr_lost; CROSWatchdog watchdog_qr_lost;
...@@ -152,7 +157,7 @@ class AutomaticChargeAlgNode : public algorithm_base::IriBaseAlgorithm<Automatic ...@@ -152,7 +157,7 @@ class AutomaticChargeAlgNode : public algorithm_base::IriBaseAlgorithm<Automatic
// void process_data(void); // void process_data(void);
bool active, active2; bool active, active2, active3, find_int_point, no_int_sol, send, second_try;
/** /**
* \brief config variable * \brief config variable
...@@ -220,6 +225,7 @@ class AutomaticChargeAlgNode : public algorithm_base::IriBaseAlgorithm<Automatic ...@@ -220,6 +225,7 @@ class AutomaticChargeAlgNode : public algorithm_base::IriBaseAlgorithm<Automatic
// [test functions] // [test functions]
void config_smart_charger(bool enable, bool disable, double period, double limit_current); void config_smart_charger(bool enable, bool disable, double period, double limit_current);
void qr_size(double qr_x, double qr_y);
}; };
#endif #endif
automatic_charge/launch/automatic_charge_sim.launch
+ 11
− 5
View file @ a1d65d0e
...@@ -20,17 +20,23 @@ ...@@ -20,17 +20,23 @@
<param name="search_module/joint_trajectory/enable_timeout" value="false"/> <param name="search_module/joint_trajectory/enable_timeout" value="false"/>
<param name="search_module/joint_trajectory/rate_hz" value="10.0"/> <param name="search_module/joint_trajectory/rate_hz" value="10.0"/>
<param name="joint_trajectory/rate_hz" value="5.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="walk/cmd_vel_rate_hz" value="50.0"/>
<param name="tracking_module/rate_hz" value="6"/> <param name="tracking_module/rate_hz" value="4"/>
<param name="tracking_module/head_tracking/rate_hz" value="10.0"/> <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="search_module/rate_hz" value="6"/>
<param name="action_module/rate_hz" value="10"/>
<param name="action_module/enable_timeout" value="false"/> <param name="action_module/enable_timeout" value="false"/>
<param name="joints_cart_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/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" <remap from="/darwin/automatic_charge/qr_pose"
to="/qr_detector/qr_pose"/> to="/qr_detector/qr_pose"/>
<remap from="/darwin/automatic_charge/set_qr_size"
to="/qr_detector/set_qr_size"/>
<!-- Smart charger --> <!-- Smart charger -->
<remap from="/darwin/automatic_charge/smart_charger_data" <remap from="/darwin/automatic_charge/smart_charger_data"
to="/darwin/robot/smart_charger_data"/> to="/darwin/robot/smart_charger_data"/>
...@@ -160,8 +166,8 @@ ...@@ -160,8 +166,8 @@
name="qr_detector" name="qr_detector"
type="qr_detector" type="qr_detector"
output="screen"> output="screen">
<param name="qr_x" value="0.04"/> <param name="qr_x" value="0.1"/>
<param name="qr_y" value="0.04"/> <param name="qr_y" value="0.1"/>
<param name="camera_frame" value="/darwin/darwin_cam_optical_link"/>d <param name="camera_frame" value="/darwin/darwin_cam_optical_link"/>d
<remap from="~/camera/image_raw" to="/darwin/camera/image_raw"/> <remap from="~/camera/image_raw" to="/darwin/camera/image_raw"/>
<remap from="~/camera/camera_info" to="/darwin/camera/camera_info"/> <remap from="~/camera/camera_info" to="/darwin/camera/camera_info"/>
......
automatic_charge/src/automatic_charge_alg_node.cpp
+ 118
− 49
View file @ a1d65d0e
...@@ -65,9 +65,11 @@ AutomaticChargeAlgNode::AutomaticChargeAlgNode(void) : ...@@ -65,9 +65,11 @@ AutomaticChargeAlgNode::AutomaticChargeAlgNode(void) :
// [init services] // [init services]
// [init clients] // [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] // [init action servers]
...@@ -86,8 +88,19 @@ AutomaticChargeAlgNode::AutomaticChargeAlgNode(void) : ...@@ -86,8 +88,19 @@ AutomaticChargeAlgNode::AutomaticChargeAlgNode(void) :
this->charged=false; this->charged=false;
this->discharged=false; this->discharged=false;
this->active=false; this->active=true; //envia primer punto cart
this->active2=false; 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) AutomaticChargeAlgNode::~AutomaticChargeAlgNode(void)
...@@ -112,6 +125,10 @@ void AutomaticChargeAlgNode::mainNodeThread(void) ...@@ -112,6 +125,10 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
// disable smart charger // disable smart charger
this->config_smart_charger(false, true, DEFAULT_PERIOD, DEFAULT_CURRENT); this->config_smart_charger(false, true, DEFAULT_PERIOD, DEFAULT_CURRENT);
// stop search // 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(); this->search_module.stop_search();
// stop tracking // stop tracking
this->tracking_module.stop_tracking(); this->tracking_module.stop_tracking();
...@@ -122,6 +139,10 @@ void AutomaticChargeAlgNode::mainNodeThread(void) ...@@ -122,6 +139,10 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
this->state=IDLE; this->state=IDLE;
this->status=CANCELED; this->status=CANCELED;
this->stop=false; this->stop=false;
//joints cart
find_int_point=false;
no_int_sol=false;
active=true; //para enviar el primer punto
} }
else else
{ {
...@@ -130,10 +151,10 @@ void AutomaticChargeAlgNode::mainNodeThread(void) ...@@ -130,10 +151,10 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
case IDLE: ROS_INFO("AutomaticChargeAlgNode: state IDLE"); case IDLE: ROS_INFO("AutomaticChargeAlgNode: state IDLE");
if(this->start) if(this->start)
{ {
this->action_module.execute(15); //this->action_module.execute(15);
// //
battery_status=CONNECTED; battery_status=CONNECTED;
this->state=SIT; this->state=SOC;
// //
// this->state=SOC; // this->state=SOC;
this->status=RUNNING; this->status=RUNNING;
...@@ -149,8 +170,6 @@ void AutomaticChargeAlgNode::mainNodeThread(void) ...@@ -149,8 +170,6 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
if(battery_status==DISCONNECTED) if(battery_status==DISCONNECTED)
{ {
ROS_INFO("AutomaticChargeAlgNode: Batteries disconnected!"); ROS_INFO("AutomaticChargeAlgNode: Batteries disconnected!");
// this->state=SEARCH;
// this->search_module.start_search("W1", config_.angle_rotation, config_.error);
this->state=IDLE; this->state=IDLE;
} }
if(battery_status==CONNECTED) if(battery_status==CONNECTED)
...@@ -159,14 +178,13 @@ void AutomaticChargeAlgNode::mainNodeThread(void) ...@@ -159,14 +178,13 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
if(this->discharged) if(this->discharged)
{ {
ROS_INFO("AutomaticChargeAlgNode: Batteries discharged"); ROS_INFO("AutomaticChargeAlgNode: Batteries discharged");
// this->state=SEARCH; this->qr_size(0.1,0.1); //Change QR size to detect large QR
// this->search_module.start_search("W1", config_.angle_rotation, config_.error); this->search_module.start_search(qr_id, 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=SEARCH;
this->state=TRACK;
} }
}
else else
this->state=SOC; this->state=SOC;
}
if(battery_status==CHARGING) if(battery_status==CHARGING)
{ {
ROS_INFO("AutomaticChargeAlgNode: Batteries connected and charging"); ROS_INFO("AutomaticChargeAlgNode: Batteries connected and charging");
...@@ -198,7 +216,7 @@ void AutomaticChargeAlgNode::mainNodeThread(void) ...@@ -198,7 +216,7 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
switch(this->search_module.get_status()) switch(this->search_module.get_status())
{ {
case SEARCH_MODULE_SUCCESS: ROS_INFO("AutomaticChargeAlgNode: Search completed successfully!"); 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; this->state=TRACK;
break; break;
case SEARCH_MODULE_CANCELED: ROS_INFO("AutomaticChargeAlgNode: Search canceled"); case SEARCH_MODULE_CANCELED: ROS_INFO("AutomaticChargeAlgNode: Search canceled");
...@@ -246,9 +264,8 @@ void AutomaticChargeAlgNode::mainNodeThread(void) ...@@ -246,9 +264,8 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
this->state=IDLE; this->state=IDLE;
break; break;
case TRACK_MODULE_LOST: ROS_INFO("AutomaticChargeAlgNode: QR lost!"); 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->state=SEARCH;
this->status=CANCELED;
break; break;
case TRACK_MODULE_HEAD_ERROR: ROS_INFO("AutomaticChargeAlgNode: Track failed due to error in head_tracking submodule"); case TRACK_MODULE_HEAD_ERROR: ROS_INFO("AutomaticChargeAlgNode: Track failed due to error in head_tracking submodule");
this->state=END; this->state=END;
...@@ -274,17 +291,20 @@ void AutomaticChargeAlgNode::mainNodeThread(void) ...@@ -274,17 +291,20 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
this->state=SIT; this->state=SIT;
else else
{ {
ROS_INFO("AutomaticChargeAlgNode: End of moving head"); //ROS_INFO("AutomaticChargeAlgNode: End of moving head");
if(this->action_module.get_status()==MOTION_MODULE_SUCCESS) if(this->action_module.get_status()==MOTION_MODULE_SUCCESS)
{ {
ROS_INFO("AutomaticChargeAlgNode: Moved head completed successfully"); //ROS_INFO("AutomaticChargeAlgNode: Moved head completed successfully");
//Mover cabeza (para ver los QR pequeños) + abrir grippers this->qr_size(0.035,0.035); //Change QR size for detection of small QR
this->search_module.start_search("W1R", config_.angle_rotation, config_.error, 0.8); this->qr_id.resize(4);
//mover brazo a posicion conocida 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.add_goal(4,-1.5,1,0.3); //move tilt
// this->joints_module.execute_goal(); // this->joints_module.execute_goal();
//Abrir gripper this->left_gripper.open(); //Open gripper
this->left_gripper.open();
this->state=MOVE_HEAD; this->state=MOVE_HEAD;
} }
else else
...@@ -305,47 +325,58 @@ void AutomaticChargeAlgNode::mainNodeThread(void) ...@@ -305,47 +325,58 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
this->search_module.start_search("W1", config_.angle_rotation, config_.error); this->search_module.start_search("W1", config_.angle_rotation, config_.error);
this->state=MOVE_HEAD; 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()*/) case MOVE_HEAD: ROS_INFO("AutomaticChargeAlgNode: state MOVE_HEAD");
this->state=MOVE_HEAD; if(qr_detected/* || this->search_module.is_finished() *//*!this->joints_module.is_finished() && !this->left_gripper.is_finished()*/)
else
{ {
find_int_point=true;
//active=true; //envia primer punto (2)
//Llamar a la funcion que calculara el punto al que ir //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.x=-0.0616414;
// this->gripper_position.point.y=-0.0426087; // this->gripper_position.point.y=-0.0426087;
// this->gripper_position.point.z=0.0220413; // this->gripper_position.point.z=0.0220413;
// this->gripper_position.point.z=0.000000; // this->gripper_position.point.z=0.000000;
this->state=MOVE_HEAD; /* this->state=MOVE_HEAD;
this->active=true; this->active=true;
if(this->active2) 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.set_default_speed(1.5);
this->joints_cart_module.go_to_position(gripper_position); this->joints_cart_module.go_to_position(gripper_position);
this->state=MOVE_ARM; 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; break;
case MOVE_ARM: ROS_INFO("AutomaticChargeAlgNode: state MOVE_ARM"); case MOVE_ARM: ROS_INFO("AutomaticChargeAlgNode: state MOVE_ARM");
send=false;
if(this->joints_cart_module.is_finished()) if(this->joints_cart_module.is_finished())
{ {
//if(joints_cart_module.get_status()==JOINTS_CART_MODULE_SUCCESS) //if(joints_cart_module.get_status()==JOINTS_CART_MODULE_SUCCESS)
switch(this->joints_cart_module.get_status()) switch(this->joints_cart_module.get_status())
{ {
case JOINTS_CART_MODULE_SUCCESS: ROS_INFO("AutomaticChargeAlgNode: Moved arm successfully"); case JOINTS_CART_MODULE_SUCCESS: ROS_INFO("AutomaticChargeAlgNode: Moved arm successfully");
//Cerrar gripper //Close gripper
this->left_gripper.close(); this->left_gripper.close();
this->state=CONNECT; this->state=CONNECT;
break; break;
case JOINTS_CART_MOUDLE_GET_IK_FAIL: ROS_INFO("AutomaticChargeAlgNode: Failed to move arm"); case JOINTS_CART_MOUDLE_GET_IK_FAIL: ROS_INFO("AutomaticChargeAlgNode: Failed to move arm");
//Volver a posicionarse
this->state=MOVE_HEAD; this->state=MOVE_HEAD;
active2=true; //send second point
break; break;
} }
} }
...@@ -364,12 +395,10 @@ void AutomaticChargeAlgNode::mainNodeThread(void) ...@@ -364,12 +395,10 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
if(this->battery_status==CHARGING) if(this->battery_status==CHARGING)
{ {
ROS_INFO("Batteries connected!"); ROS_INFO("Batteries connected!");
//Leer smart charger hasta que este cargado this->state=SOC; //Read smart charger data until its charged
this->state=SOC;
} }
else if(this->battery_status==CONNECTED) else if(this->battery_status==CONNECTED)
//Volver a intentar conectarse //Try to connect again
//this->state=MOVE_ARM;
this->state=MOVE_ARM; this->state=MOVE_ARM;
} }
break; break;
...@@ -382,7 +411,6 @@ void AutomaticChargeAlgNode::mainNodeThread(void) ...@@ -382,7 +411,6 @@ void AutomaticChargeAlgNode::mainNodeThread(void)
break; break;
case END: ROS_INFO("AutomaticChargeAlgNode: state END"); //stop all modules case END: ROS_INFO("AutomaticChargeAlgNode: state END"); //stop all modules
//posibles errores, estado etc
// disable smart charger // disable smart charger
this->config_smart_charger(false, true, DEFAULT_PERIOD, DEFAULT_CURRENT); this->config_smart_charger(false, true, DEFAULT_PERIOD, DEFAULT_CURRENT);
// stop search // stop search
...@@ -468,6 +496,7 @@ void TrackChargeStationAlgNode::tf_cam_shoulder(void) ...@@ -468,6 +496,7 @@ void TrackChargeStationAlgNode::tf_cam_shoulder(void)
/* [subscriber callbacks] */ /* [subscriber callbacks] */
void AutomaticChargeAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_pose_array::ConstPtr& msg) 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"); //ROS_INFO("CSearchModule::qr_pose_callback: New Message Received");
unsigned int i; unsigned int i;
double z; double z;
...@@ -477,10 +506,11 @@ void AutomaticChargeAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_po ...@@ -477,10 +506,11 @@ void AutomaticChargeAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_po
if(msg->tags.size()>0) 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 source_frame="darwin/left_shoulder";
std::string target_frame="station_connector_link"; std::string target_frame="station_connector_link";
...@@ -536,8 +566,8 @@ void AutomaticChargeAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_po ...@@ -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("Y1: %f, X1: %f",y1, x1);
ROS_INFO("Y2: %f, X2: %f",y2,x2); ROS_INFO("Y2: %f, X2: %f",y2,x2);
}else no_int_sol=true;
ROS_ERROR("AutomaticChargeAlgNode: Not found solution");
/////////// ///////////
...@@ -558,6 +588,20 @@ void AutomaticChargeAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_po ...@@ -558,6 +588,20 @@ void AutomaticChargeAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_po
tf_listener.transformPose(target_frame, in, out); 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 ); 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.frame_id = source_frame;
in.header.stamp = target_time; in.header.stamp = target_time;
in.pose.position.x = x1; in.pose.position.x = x1;
...@@ -572,14 +616,27 @@ void AutomaticChargeAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_po ...@@ -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.y=out.pose.position.y; //respecto el disco
this->gripper_position.point.x=out.pose.position.x; this->gripper_position.point.x=out.pose.position.x;
this->gripper_position.point.z=out.pose.position.z; 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->joints_cart_module.set_default_speed(1.5);
this->active2=true; 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 ...@@ -590,12 +647,12 @@ void AutomaticChargeAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_po
} }
}
// qr_detected=true; // qr_detected=true;
this->watchdog_qr_lost.reset(ros::Duration(1)); this->watchdog_qr_lost.reset(ros::Duration(1));
}else{ }else{
ROS_WARN("QR NOT DETECTED"); ROS_INFO("AutomaticChargeAlgNode:: QR NOT Detected");
} }
this->qr_pose_access.exit(); this->qr_pose_access.exit();
...@@ -879,6 +936,18 @@ void AutomaticChargeAlgNode::process_data(void) ...@@ -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) void AutomaticChargeAlgNode::config_smart_charger(bool enable, bool disable, double period, double limit_current)
{ {
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment