diff --git a/darwin_apps/launch/joint_trajectory_client.launch b/darwin_apps/launch/joint_trajectory_client.launch
index ebbd503abf45ce24f7be35a7f986651fea07a9b9..839e8e3ccb21727bf7f1316a062c701ca2d94453 100644
--- a/darwin_apps/launch/joint_trajectory_client.launch
+++ b/darwin_apps/launch/joint_trajectory_client.launch
@@ -12,10 +12,12 @@
type="joints_client"
output="screen"
ns="/darwin">
- <remap from="/darwin/joint_trajectory"
+ <remap from="/darwin/joints_client/joint_trajectory/joint_trajectory_action"
to="/darwin/robot/joint_trajectory"/>
- <remap from="/darwin/joints_client/set_servo_modules"
+ <remap from="/darwin/joints_client/joint_trajectory/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
+ <remap from="/darwin/joints_client/joint_trajectory/joint_state"
+ to="/darwin/robot/joint_state"/>
</node>
<!-- launch dynamic reconfigure -->
@@ -24,3 +26,4 @@
</launch>
+
diff --git a/darwin_driver/src/darwin_driver.cpp b/darwin_driver/src/darwin_driver.cpp
index 57f86e396718df795096daf6e0aa89ae5a1950dd..c73ae06e2454ad1d861e6a768b4489c85d9e5a02 100644
--- a/darwin_driver/src/darwin_driver.cpp
+++ b/darwin_driver/src/darwin_driver.cpp
@@ -80,12 +80,12 @@ bool DarwinDriver::startDriver(void)
if(!this->darwin->mm_is_running())
this->darwin->mm_start();
// move the robot to the walk ready position
- ROS_INFO("Moving to the walk ready position ...");
- this->darwin->action_load_page(WALK_READY);
- this->darwin->action_start();
- while(this->darwin->action_is_page_running())
- usleep(100000);
- ROS_INFO("Robot at the walk ready position");
+// ROS_INFO("Moving to the walk ready position ...");
+// this->darwin->action_load_page(WALK_READY);
+// this->darwin->action_start();
+// while(this->darwin->action_is_page_running())
+// usleep(100000);
+// ROS_INFO("Robot at the walk ready position");
// calibrate the gyro and and enable balancing
this->darwin->imu_start();
if(this->darwin->imu_is_accel_detected())
diff --git a/sm_qr_search/CMakeLists.txt b/sm_qr_search/CMakeLists.txt
index 216f38856af7bf3273f272af82c65cf0521f76df..38fe668e76acffb043985e2aa5370d6ebcb6e09e 100644
--- a/sm_qr_search/CMakeLists.txt
+++ b/sm_qr_search/CMakeLists.txt
@@ -6,7 +6,7 @@ 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)
+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 humanoid_modules)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -60,7 +60,7 @@ catkin_package(
# ********************************************************************
# 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
+ CATKIN_DEPENDS iri_base_algorithm nav_msgs geometry_msgs trajectory_msgs control_msgs humanoid_common_msgs actionlib sensor_msgs iri_ros_tools tf humanoid_modules
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
diff --git a/sm_qr_search/include/sm_qr_search_alg_node.h b/sm_qr_search/include/sm_qr_search_alg_node.h
index 33b9fb642ef0d199c9d30644248c7ad95fb94dcf..78f4e36f069d671f8a66be0440ed5c78166dbe16 100644
--- a/sm_qr_search/include/sm_qr_search_alg_node.h
+++ b/sm_qr_search/include/sm_qr_search_alg_node.h
@@ -48,8 +48,12 @@
#include <actionlib/client/terminal_state.h>
#include <humanoid_common_msgs/humanoid_follow_targetAction.h>
+//MODULES
+#include <humanoid_modules/joints_module.h>
+
+
//states
-typedef enum {IDLE, POS_INIT, SEARCH, TRACK_QR, ROTATE, WAIT_ROTATE} States;
+typedef enum {IDLE, START, SEARCH, TRACK_QR, ROTATE, WAIT_ROTATE} States;
/**
* \brief IRI ROS Specific Algorithm Class
@@ -99,12 +103,12 @@ class SmQrSearchAlgNode : public algorithm_base::IriBaseAlgorithm<SmQrSearchAlgo
// [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<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_;
@@ -138,6 +142,18 @@ class SmQrSearchAlgNode : public algorithm_base::IriBaseAlgorithm<SmQrSearchAlgo
CROSWatchdog watchdog_qr_lost;
//CROSWatchdog watchdog_walk;
//CROSWatchdog feedback_watchdog;
+ /*
+ unsigned int b[2];
+ double a[2];
+ std::vector<double> speed;
+ std::vector<double> accel; //2 elements with value 0.3
+ std::vector<double> angle;
+ std::vector<unsigned int> id;
+ */
+ ///JOINTS MODULE
+ CJointsModule joints_trajectory_module;
+ std::vector<double> angle_feedback;
+
/**
* \brief config variable
diff --git a/sm_qr_search/package.xml b/sm_qr_search/package.xml
index 3f573e4fe30f930c160a701baca0ddaca1561582..0af94b8067932d9d2483adc7234cb5e75b19f0cb 100644
--- a/sm_qr_search/package.xml
+++ b/sm_qr_search/package.xml
@@ -50,6 +50,7 @@
<build_depend>sensor_msgs</build_depend>
<build_depend>iri_ros_tools</build_depend>
<build_depend>tf</build_depend>
+ <build_depend>humanoid_modules</build_depend>
<run_depend>iri_base_algorithm</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
@@ -60,6 +61,7 @@
<run_depend>sensor_msgs</run_depend>
<run_depend>iri_ros_tools</run_depend>
<run_depend>tf</run_depend>
+ <run_depend>humanoid_modules</run_depend>
<!-- The export tag contains other, unspecified, tags -->
diff --git a/sm_qr_search/sm_qr_search.launch b/sm_qr_search/sm_qr_search.launch
index e62af4d123b4a0062bf44b9d532e906923d74341..dffbf1eaef7d26a34e339ffb2643c847937a9e3f 100644
--- a/sm_qr_search/sm_qr_search.launch
+++ b/sm_qr_search/sm_qr_search.launch
@@ -41,6 +41,8 @@
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/joint_trajectory/set_servo_modules"
+ to="/darwin/robot/set_servo_modules"/>
<remap from="/darwin/sm_qr_search/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
<remap from="/darwin/head_follow_target"
@@ -51,7 +53,7 @@
to="/darwin/joint_states"/>
<remap from="/darwin/sm_qr_search/qr_pose"
to="/qr_detector/qr_pose"/>
- <remap from="/darwin/joint_trajectory"
+ <remap from="/darwin/sm_qr_search/joint_trajectory/joint_trajectory_action"
to="/darwin/robot/joint_trajectory"/>
</node>
diff --git a/sm_qr_search/src/sm_qr_search_alg_node.cpp b/sm_qr_search/src/sm_qr_search_alg_node.cpp
index 6ea959bbec09055bf4ad103fcb3eb8802ab1f9f4..cf106bb11941aaa1514276e0de018c3daee16d1d 100644
--- a/sm_qr_search/src/sm_qr_search_alg_node.cpp
+++ b/sm_qr_search/src/sm_qr_search_alg_node.cpp
@@ -10,6 +10,7 @@
#include "sm_qr_search_alg_node.h"
+/*
#define NUM_SERVOS 32
// joint names
const std::string servo_names[NUM_SERVOS]={std::string("Servo0"),
@@ -45,9 +46,11 @@ const std::string servo_names[NUM_SERVOS]={std::string("Servo0"),
std::string("Servo30"),
std::string("Servo31")};
+ */
SmQrSearchAlgNode::SmQrSearchAlgNode(void) :
algorithm_base::IriBaseAlgorithm<SmQrSearchAlgorithm>(),
- joint_trajectory_client_("joint_trajectory", true),
+ //joint_trajectory_client_("joint_trajectory", true),
+ joints_trajectory_module("joint_trajectory"),
head_follow_target_client_("head_follow_target", true)
{
//init class attributes if necessary
@@ -101,7 +104,7 @@ SmQrSearchAlgNode::SmQrSearchAlgNode(void) :
this->counter=0;
- actionlib::SimpleClientGoalState joint_trajectory_state(actionlib::SimpleClientGoalState::PENDING);
+ //actionlib::SimpleClientGoalState joint_trajectory_state(actionlib::SimpleClientGoalState::PENDING);
//head_follow
@@ -109,17 +112,24 @@ SmQrSearchAlgNode::SmQrSearchAlgNode(void) :
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->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);
-
+
+/* this->b={19,20};
+ this->a={-1,0.8};
+ this->speed(2,1);
+ this->accel (2,0.3); //2 elements with value 0.3
+ this->angle (a, a + sizeof(a) / sizeof(double) );
+ this->id (b, b + sizeof(b) / sizeof(unsigned int) );
+ */
}
SmQrSearchAlgNode::~SmQrSearchAlgNode(void)
@@ -215,7 +225,19 @@ void SmQrSearchAlgNode::mainNodeThread(void)
*/
-
+ unsigned int b[2]={19,20};
+ double a[2]={-1.5,0.8};
+ std::vector<double> speed(2,1);
+ std::vector<double> accel(2,0.3); //2 elements with value 0.3
+ std::vector<double> angle(a, a + sizeof(a) / sizeof(double) );
+ std::vector<unsigned int> id(b, b + sizeof(b) / sizeof(unsigned int) );
+
+/* unsigned int id[2]={19,20};
+ double angle[2]={-1,0.8};
+ double speed[2]={1,1};
+ double accel[2]={0.3};
+ */
+
switch(state)
{
case IDLE:
@@ -223,33 +245,44 @@ void SmQrSearchAlgNode::mainNodeThread(void)
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;
- }
+ ROS_INFO("Start");
+ new_goal=false;
+// 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;
+// }
+ if(this->joints_trajectory_module.execute(id,angle,speed,accel))
+ //if(this->joints_trajectory_module.get_status()==JOINTS_MODULE_RUNNING)
+ state=START;
+ // state=START;
}
else
state=IDLE;
break;
//Wait joint trajectory to execute
- case POS_INIT:
- //ROS_INFO("POS_INIT");
- if(executing_trajectory){
+ case START:
+ ROS_INFO("Start Search");
+ //if(!this->joints_trajectory_module.is_finished()){
+ if(this->joints_trajectory_module.get_status()==JOINTS_MODULE_RUNNING){
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;
- }
+ state=START;
+
+ }else{
+ angle[0]=1.5; //Change pan to end
+ if(this->joints_trajectory_module.execute(id,angle,speed,accel))
+ state=SEARCH;
+
+ // joint_trajectory_set_goal(pan_angle_end, tilt_angle_search);
+ // if(joint_trajectory_start_action())
+ // {
+ // ROS_INFO("Start search");
+ // state=SEARCH;
+ //}else
+ //ROS_INFO("New trajectory canceled");
}
break;
@@ -258,75 +291,47 @@ void SmQrSearchAlgNode::mainNodeThread(void)
ROS_INFO("SEARCH");
if(qr_detected){
ROS_INFO("QR detected");
- this->joint_trajectory_client_.cancelGoal();
+ this->joints_trajectory_module.stop();
+ //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);
+ set_head_tracking_goal(this->current_pan_angle, this->current_tilt_angle);
+ //set_head_tracking_goal(angle_feedback[0], angle_feedback[1]);
if(head_follow_targetMakeActionRequest())
{
state=TRACK_QR;
}
}else{
- if(!executing_trajectory){
+ if(this->joints_trajectory_module.is_finished()){
ROS_INFO("End of search");
state=IDLE;
}else{
- ROS_INFO("current pan angle: %f", this->current_pan_angle);
+ angle_feedback=this->joints_trajectory_module.get_current_angle();
+ //std::cout << "Current pan: " << angle_feedback[0] << std::endl;
+ //std::cout << "Current tilt: " <<angle_feedback[1] << std::endl;
+ //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)
+ if(this->watchdog_qr_lost.is_active())
{
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())
- {
+ angle[0]=-1.5;
+ //this->joints_trajectory_module.execute_goal(19,-1,1,0.3);
+ if( this->joints_trajectory_module.execute(id, angle, speed, accel))
+// 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;
- }
+ state=START;
+ // }
}
//}
else if(end)
@@ -390,13 +395,13 @@ void SmQrSearchAlgNode::mainNodeThread(void)
nav_module->nav_timeout.stop();
// set the error
nav_module->state=HANDLE_ERROR;
- }
+ }joint_states
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();
+ this->move_base_client->cancelGoal();joint_states
nav_module->nav_access.enter();
// set the error
nav_module->status=NAV_MODULE_FB_WATCHDOG;
@@ -465,7 +470,7 @@ void SmQrSearchAlgNode::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
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);
+ //ROS_INFO("Orientacion Odometria: %f", odom_rot);
//unlock previously blocked shared variables
//this->alg_.unlock();
@@ -492,23 +497,26 @@ void SmQrSearchAlgNode::qr_pose_callback(const humanoid_common_msgs::tag_pose_ar
if(msg->tags.size()>0)
{
+ ROS_INFO("QR detected");
//qr_detected=msg->tags.size();
qr_detected=true;
+ //angle_feedback=this->joints_trajectory_module.get_current_angle();
+
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));
+ 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));
+ this->watchdog_qr_lost.reset(ros::Duration(5));
counter=0;
}else{
- ROS_INFO("QR not detected");
+ //ROS_INFO("QR not detected");
qr_detected=false;
counter++;
@@ -531,7 +539,7 @@ void SmQrSearchAlgNode::qr_pose_mutex_exit(void)
{
pthread_mutex_unlock(&this->qr_pose_mutex_);
}
-
+ //Se utiliza en el joints_module
void SmQrSearchAlgNode::joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg)
{
//ROS_INFO("SmQrSearchAlgNode::joint_states_callback: New Message Received");
@@ -543,6 +551,7 @@ void SmQrSearchAlgNode::joint_states_callback(const sensor_msgs::JointState::Con
unsigned int i;
+
for (i = 0; i < msg->name.size() ; ++i){
if (msg->name[i]=="j_pan"){
this->current_pan_angle=msg->position[i];
@@ -552,6 +561,9 @@ void SmQrSearchAlgNode::joint_states_callback(const sensor_msgs::JointState::Con
}
}
+ // ROS_INFO("current pan: %f", current_pan_angle);
+ // ROS_INFO("current tilt: %f", current_tilt_angle);
+
//unlock previously blocked shared variables
//this->alg_.unlock();
this->joint_states_mutex_exit();
@@ -572,6 +584,7 @@ void SmQrSearchAlgNode::joint_states_mutex_exit(void)
/* [service callbacks] */
/* [action callbacks] */
+/*
void SmQrSearchAlgNode::joint_trajectoryDone(const actionlib::SimpleClientGoalState& state, const control_msgs::JointTrajectoryResultConstPtr& result)
{
alg_.lock();
@@ -615,7 +628,7 @@ void SmQrSearchAlgNode::joint_trajectoryFeedback(const control_msgs::JointTrajec
}
alg_.unlock();
}
-
+*/
void SmQrSearchAlgNode::head_follow_targetDone(const actionlib::SimpleClientGoalState& state, const humanoid_common_msgs::humanoid_follow_targetResultConstPtr& result)
{
alg_.lock();
@@ -656,6 +669,7 @@ void SmQrSearchAlgNode::head_follow_targetFeedback(const humanoid_common_msgs::h
/* [action requests] */
+/*
bool SmQrSearchAlgNode::joint_trajectoryMakeActionRequest()
{
// IMPORTANT: Please note that all mutex used in the client callback functions
@@ -683,7 +697,7 @@ bool SmQrSearchAlgNode::joint_trajectoryMakeActionRequest()
return false;
}
}
-
+*/
bool SmQrSearchAlgNode::head_follow_targetMakeActionRequest()
{
// IMPORTANT: Please note that all mutex used in the client callback functions
@@ -865,8 +879,8 @@ void SmQrSearchAlgNode::send_head_target(double pan, double tilt)
}
//Joint trajectory
-void SmQrSearchAlgNode::joint_trajectory_set_goal(double pan_position, double tilt_position)
-{
+//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";
@@ -883,7 +897,7 @@ void SmQrSearchAlgNode::joint_trajectory_set_goal(double pan_position, double ti
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;
@@ -895,26 +909,27 @@ void SmQrSearchAlgNode::joint_trajectory_set_goal(double pan_position, double ti
//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;
- }
-
-}
+// 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 */
diff --git a/track_charge_station/launch/sim_track_charge_station.launch b/track_charge_station/launch/sim_track_charge_station.launch
index d1f8966687bb307c164b517e611d60249a826b1f..34c2c0c6ce5123f801976cfb666e6e6cc80c1bbc 100644
--- a/track_charge_station/launch/sim_track_charge_station.launch
+++ b/track_charge_station/launch/sim_track_charge_station.launch
@@ -38,7 +38,7 @@
</node>
<!-- <include file="$(find qr_detector)/launch/tracker_darwin.launch"/> -->
-
+<<<
<node pkg="qr_detector"
name="qr_detector"
type="qr_detector"