diff --git a/cfg/IriJointsClient.cfg b/cfg/IriJointsClient.cfg
index 5c53d5a4db57c82f8457e5d4c6cd9a998600963f..784fbc3e2aa7dad5d310c797f1caa38402754963 100755
--- a/cfg/IriJointsClient.cfg
+++ b/cfg/IriJointsClient.cfg
@@ -39,6 +39,7 @@ gen = ParameterGenerator()
point_to = gen.add_group("PointHead")
tracker = gen.add_group("Tracker")
+move_to_angles = gen.add_group("MoveToAngles")
# 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)
@@ -47,8 +48,8 @@ point_to.add("point_y", double_t, 0, "Po
point_to.add("point_z", double_t, 0, "Point Z coordenate", 0.0, -100.0, 100.0)
point_to.add("vel_max", double_t, 0, "Pointing maximum velocity", 0.5, -0.1, 6.28)
point_to.add("send_point_to", bool_t, 0, "Call point_to", False)
-point_to.add("cancel", bool_t, 0, "Cancel point_to goal", False)
-point_to.add("ik_tol", double_t, 0, "Inverse kinematics solver tolerance", 0.001, 0.0, 1.0)
+point_to.add("cancel_point_to", bool_t, 0, "Cancel point_to goal", False)
+point_to.add("ik_tol", double_t, 0, "Inverse kinematics solver tolerance", 0.001, 0.0, 1.0)
point_to.add("send_ik_tol", bool_t, 0, "Send inverse kinematic tolerance", False)
point_to.add("base_frame", str_t, 0, "Kinematic chain base frame", "servo1")
point_to.add("send_base_frame", bool_t, 0, "Send Kinematic chain base frame", False)
@@ -57,6 +58,15 @@ point_to.add("send_urdf_param", bool_t, 0, "Se
tracker.add("start_tracking", bool_t, 0, "Start tracking action", False)
tracker.add("stop_tracking", bool_t, 0, "Cancel tracking action", False)
-tracker.add("update_target", bool_t, 0, "Cancel tracking action", False)
+tracker.add("update_target", bool_t, 0, "Update target", False)
+
+move_to_angles.add("joint_name", str_t, 0, "Joint's name", "pan")
+move_to_angles.add("joint_angle", double_t, 0, "Joint's desired angle", 0.0, -3.14, 3.14)
+move_to_angles.add("joint_vel", double_t, 0, "Joint's desired velocity", 0.0, -100.0, 100.0)
+move_to_angles.add("add_joint", bool_t, 0, "Add joint's name, angle and velocity", False)
+move_to_angles.add("send_move_to_angles", bool_t, 0, "Start MoveToAngles action", False)
+move_to_angles.add("cancel_move_to_angles", bool_t, 0, "Cancel MoveToAngles action", False)
+# move_to_angles.add("move_to_angles_time_from_start", int_t, 0, "Time from start", 10, 0, 100)
+# move_to_angles.add("send_time_from_start", bool_t, 0, "Send time from start", False)
exit(gen.generate(PACKAGE, "IriJointsClientAlgorithm", "IriJointsClient"))
diff --git a/cfg/IriJointsModule.cfg b/cfg/IriJointsModule.cfg
index a98a607c7865f43740bf61505895b9cd19858c9d..bd1bebbfa4e7665a744df39cca9be442ee41dd5a 100755
--- a/cfg/IriJointsModule.cfg
+++ b/cfg/IriJointsModule.cfg
@@ -39,6 +39,7 @@ gen = ParameterGenerator()
joint_states = gen.add_group("JointStates")
point_to = gen.add_group("PointHead")
tracker = gen.add_group("Tracker")
+move_to_angles = gen.add_group("MoveToAngles")
axis_enum = gen.enum([ gen.const("x", int_t, 0, "Pointing axis x"),
gen.const("y", int_t, 1, "Pointing axis y"),
@@ -49,6 +50,7 @@ axis_enum = gen.enum([ gen.const("x", int_t, 0, "Pointing axis x"),
# 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("rate_hz", double_t, 0, "Navigation module rate in Hz", 1.0, 1.0, 100.0)
+gen.add("frame_id", str_t, 0, "Joints module frame id", "joints_module")
joint_states.add("joint_states_watchdog_time_s", double_t, 0, "Maximum time between JointStates messages",1, 0.01, 50)
@@ -68,5 +70,12 @@ tracker.add("tracker_feedback_watchdog_time_s", double_t, 0,
tracker.add("tracker_enable_timeout", bool_t, 0, "Enable action timeout", True)
tracker.add("tracker_timeout_s", double_t, 0, "Maximum time allowed to complete the action", 5, 0.1, 6000)
+move_to_angles.add("move_to_angles_time_from_start", int_t, 0, "Max time in seconds to finish the action", 10, 0, 100)
+
+move_to_angles.add("move_to_angles_max_retries", int_t, 0, "Maximum number of retries for the action start", 1, 1, 10)
+move_to_angles.add("move_to_angles_enable_watchdog", bool_t, 0, "Enable action watchdog", True)
+move_to_angles.add("move_to_angles_feedback_watchdog_time_s", double_t, 0, "Maximum time between feedback messages", 0.5, 0.01, 600)
+move_to_angles.add("move_to_angles_enable_timeout", bool_t, 0, "Enable action timeout", True)
+move_to_angles.add("move_to_angles_timeout_s", double_t, 0, "Maximum time allowed to complete the action", 5, 0.1, 6000)
exit(gen.generate(PACKAGE, "IriJointsModuleAlgorithm", "IriJointsModule"))
diff --git a/config/joints_module_default.yaml b/config/joints_module_default.yaml
index d9765fe6375d2255fe0121c114461267edb05580..8d762fdb6dc6a98d45788a4299fb8102c1c9d94a 100644
--- a/config/joints_module_default.yaml
+++ b/config/joints_module_default.yaml
@@ -1,4 +1,5 @@
rate_hz: 10.0
+frame_id: joints_module
joint_states_watchdog_time_s: 1.0
@@ -17,3 +18,11 @@ tracker_enable_watchdog: True
tracker_feedback_watchdog_time_s: 1.0
tracker_enable_timeout: True
tracker_timeout_s: 10
+
+move_to_angles_time_from_start: 10
+
+move_to_angles_max_retries: 1
+move_to_angles_enable_watchdog: True
+move_to_angles_feedback_watchdog_time_s: 1.0
+move_to_angles_enable_timeout: True
+move_to_angles_timeout_s: 20
diff --git a/include/iri_joints_client_alg_node.h b/include/iri_joints_client_alg_node.h
index 19b05b47018bed501fa4a4cd3ae2209f4f182334..ee4ec750785d366325f4b8d7adac80219d404da4 100644
--- a/include/iri_joints_client_alg_node.h
+++ b/include/iri_joints_client_alg_node.h
@@ -56,6 +56,9 @@ class IriJointsClientAlgNode : public algorithm_base::IriBaseAlgorithm<IriJoints
// [nav client attributes]
CIriJointsModule joints_module;
+ std::vector<std::string> joint_names;
+ std::vector<double> angles;
+ std::vector<double> vel;
public:
/**
diff --git a/include/iri_joints_module/iri_joints_module.h b/include/iri_joints_module/iri_joints_module.h
index 9a4eef19b39c3eb2761f2eb40934e3dd22888e06..56ba932c4b63f26372929aeccba0f973db88f753 100644
--- a/include/iri_joints_module/iri_joints_module.h
+++ b/include/iri_joints_module/iri_joints_module.h
@@ -13,6 +13,7 @@
// ROS headers
#include <control_msgs/PointHeadAction.h>
#include <control_msgs/PointHeadActionGoal.h>
+#include <control_msgs/FollowJointTrajectoryAction.h>
#include <sensor_msgs/JointState.h>
#include <iri_joints_msgs/pointHeadTrackerAction.h>
@@ -20,7 +21,7 @@
#include <iri_joints_module/IriJointsModuleConfig.h>
//States
-typedef enum {IRI_JOINTS_MODULE_IDLE, IRI_JOINTS_MODULE_POINT_WAIT, IRI_JOINTS_MODULE_TRACKING} iri_joints_module_state_t;
+typedef enum {IRI_JOINTS_MODULE_IDLE, IRI_JOINTS_MODULE_POINT_WAIT, IRI_JOINTS_MODULE_TRACKING, IRI_JOINTS_MODULE_MOVE_ANGLES} iri_joints_module_state_t;
//Status
/**
@@ -135,6 +136,13 @@ class CIriJointsModule : public CModule<iri_joints_module::IriJointsModuleConfig
ros::Publisher tracker_point_to_publisher_;
control_msgs::PointHeadActionGoal target_goal;
+
+ //FollowJointTrayectoryAction
+ CModuleAction<control_msgs::FollowJointTrajectoryAction> move_to_angle_action;
+ control_msgs::FollowJointTrajectoryGoal move_to_angle_goal;
+
+ bool new_angles;
+ bool cancel_move_to_angles_pending;
//Variables
/**
* \brief
@@ -231,6 +239,33 @@ class CIriJointsModule : public CModule<iri_joints_module::IriJointsModuleConfig
*/
bool is_tracking_finished(void);
+ /*Move to angles*/
+ /**
+ * \brief Function to move the joints to a desired angles at a desired velocities.
+ *
+ * \param joint_names String with the names ogf each joint.
+ *
+ * \param angles Desired angle for each joint in the same order than in joint_names.
+ *
+ * \param vel Cesired velocities for each joint in the same order than in joint_names.
+ *
+ * \return True if action is called.
+ */
+ bool move_to_angles(std::vector<std::string> joint_names, std::vector<double> angles, std::vector<double> vel);
+
+ /**
+ * \brief Function to cancel point_to action.
+ *
+ */
+ void cancel_move_to_angles(void);
+
+ /**
+ * \brief Function to know if point to action is finished.
+ *
+ * \return True if finished.
+ */
+ bool is_move_to_angles_finished(void);
+
/*Common*/
/**
* \brief Function to change the inverse kinematics solver tolerance.
@@ -258,6 +293,7 @@ class CIriJointsModule : public CModule<iri_joints_module::IriJointsModuleConfig
* \return True if succeeded.
*/
bool set_urdf_param(std::string &urdf_param);
+
//Change?; Problem dynamic reconfigure module, solution param out of DynRec
//void set_point_frame(std::string &point_frame);
//Change?; Problem dynamic reconfigure module, solution param out of DynRec
diff --git a/launch/joints_client.launch b/launch/joints_client.launch
index 37dfcc62f2fa8981a42a960c8f3b35c35fb98931..315d9d708e3a42370bb2c760dc3ede045dce96a5 100644
--- a/launch/joints_client.launch
+++ b/launch/joints_client.launch
@@ -22,6 +22,8 @@
to="/point_to/track"/>
<remap from="~/joints_module/tracker_point_to"
to="/point_to/tracker_point_to"/>
+ <remap from="~/joints_module/move_to_angle"
+ to="/three_servos/effort_controller/follow_joint_trajectory"/>
<rosparam file="$(arg config_file)" command="load" ns="joints_module" />
</node>
diff --git a/launch/joints_client_bt.launch b/launch/joints_client_bt.launch
index 7164a1af71cb2a4e6a6687f22949d0e78357a043..28cb28c780e65de7daee82f09f2e3e2f4d5e5eb7 100644
--- a/launch/joints_client_bt.launch
+++ b/launch/joints_client_bt.launch
@@ -22,8 +22,12 @@
to="/three_servos/joint_states"/>
<remap from="~/joints_module/point_to_reconf"
to="/point_to/set_parameters"/>
- <remap from="~/joints_module/set_tracker"
- to="/point_to/set_tracker"/>
+ <remap from="~/joints_module/track"
+ to="/point_to/track"/>
+ <remap from="~/joints_module/tracker_point_to"
+ to="/point_to/tracker_point_to"/>
+ <remap from="~/joints_module/move_to_angle"
+ to="/three_servos/effort_controller/follow_joint_trajectory"/>
<param name="xml_path" value="$(arg xml_path)"/>
<param name="xml_file" value="$(arg xml_file)"/>
<param name="send_ticks" value="$(arg send_ticks)"/>
diff --git a/src/iri_joints_client_alg_node.cpp b/src/iri_joints_client_alg_node.cpp
index 8ee115ebc28af9f48a9ecbcebd6fa05af1b0ab26..141058723b7ff6bef9f265e8c11cc9a1f03e9625 100644
--- a/src/iri_joints_client_alg_node.cpp
+++ b/src/iri_joints_client_alg_node.cpp
@@ -14,6 +14,9 @@ IriJointsClientAlgNode::IriJointsClientAlgNode(void) :
// [init services]
// [init clients]
+ std::vector<std::string>().swap(this->joint_names);
+ std::vector<double>().swap(this->angles);
+ std::vector<double>().swap(this->vel);
}
IriJointsClientAlgNode::~IriJointsClientAlgNode(void)
@@ -43,7 +46,10 @@ void IriJointsClientAlgNode::node_config_update(Config &config, uint32_t level)
if (config.send_point_to)
{
config.send_point_to = false;
- this->joints_module.point_to(config.point_x, config.point_y, config.point_z, config.vel_max);
+ if (this->joints_module.point_to(config.point_x, config.point_y, config.point_z, config.vel_max))
+ ROS_DEBUG("Point_to called");
+ else
+ ROS_ERROR("Error while calling point_to");
}
if (config.send_ik_tol)
{
@@ -54,9 +60,9 @@ void IriJointsClientAlgNode::node_config_update(Config &config, uint32_t level)
else
ROS_ERROR("Error while setting tolerance");
}
- if(config.cancel)
+ if(config.cancel_point_to)
{
- config.cancel = false;
+ config.cancel_point_to = false;
ROS_DEBUG("Cancel action point_to");
this->joints_module.cancel_point_to();
}
@@ -93,10 +99,34 @@ void IriJointsClientAlgNode::node_config_update(Config &config, uint32_t level)
{
config.update_target = false;
if (this->joints_module.update_target(config.point_x, config.point_y, config.point_z))
- ROS_INFO("Update target");
+ ROS_DEBUG("Update target");
else
ROS_ERROR("Error while updating target");
}
+
+ if (config.add_joint)
+ {
+ config.add_joint = false;
+ this->joint_names.push_back(config.joint_name);
+ this->angles.push_back(config.joint_angle);
+ this->vel.push_back(config.joint_vel);
+ }
+ if (config.send_move_to_angles)
+ {
+ config.send_move_to_angles = false;
+ if (this->joints_module.move_to_angles(this->joint_names, this->angles, this->vel))
+ ROS_DEBUG("Move_to_angles called");
+ else
+ ROS_ERROR("Error while calling move_to_angles");
+ std::vector<std::string>().swap(this->joint_names);
+ std::vector<double>().swap(this->angles);
+ std::vector<double>().swap(this->vel);
+ }
+ if (config.cancel_move_to_angles)
+ {
+ config.cancel_move_to_angles = false;
+ this->joints_module.cancel_move_to_angles();
+ }
this->alg_.unlock();
}
diff --git a/src/iri_joints_module.cpp b/src/iri_joints_module.cpp
index caae2477b28cbbf709fd96185bc801a45e6c23f2..5084fab7c0eb9018f989c32467a05045e0042bba 100644
--- a/src/iri_joints_module.cpp
+++ b/src/iri_joints_module.cpp
@@ -4,7 +4,8 @@
CIriJointsModule::CIriJointsModule(const std::string &name,const std::string &name_space) : CModule(name,name_space),
point_to_action("point_to",this->module_nh.getNamespace()),
point_to_reconf("point_to_reconf",this->module_nh.getNamespace()),
- track_action("track", this->module_nh.getNamespace())
+ track_action("track", this->module_nh.getNamespace()),
+ move_to_angle_action("move_to_angle", this->module_nh.getNamespace())
{
this->start_operation();
@@ -13,6 +14,12 @@ CIriJointsModule::CIriJointsModule(const std::string &name,const std::string &na
//Variables
this->state = IRI_JOINTS_MODULE_IDLE;
this->status = IRI_JOINTS_MODULE_SUCCESS;
+ this->new_angles = false;
+ this->new_point_goal = false;
+ this->cancel_tracker_pending = false;
+ this->cancel_point_to_pending = false;
+ this->cancel_move_to_angles_pending = false;
+ this->enable_tracker_pending = false;
// initialize JointStates subscriber
this->joint_states_watchdog.reset(ros::Duration(this->config.joint_states_watchdog_time_s));
@@ -67,6 +74,29 @@ void CIriJointsModule::state_machine(void)
ROS_ERROR("CIriJointsModule: PointHeadAction goal start failed");
break;
}
+ break;
+ }
+ if (this->new_angles)
+ {
+ switch (this->move_to_angle_action.make_request(this->move_to_angle_goal))
+ {
+ case ACT_SRV_SUCCESS:
+ this->state = IRI_JOINTS_MODULE_MOVE_ANGLES;
+ this->new_angles = false;
+ ROS_INFO("CIriJointsModule : MoveToAngles goal start successfull");
+ break;
+ case ACT_SRV_PENDING:
+ this->state = IRI_JOINTS_MODULE_IDLE;
+ ROS_WARN("CIriJointsModule : MoveToAngles goal start pending");
+ break;
+ case ACT_SRV_FAIL:
+ this->state = IRI_JOINTS_MODULE_IDLE;
+ this->status = IRI_JOINTS_MODULE_ACTION_START_FAIL;
+ this->new_angles = false;
+ ROS_ERROR("CIriJointsModule: MoveToAngles goal start failed");
+ break;
+ }
+ break;
}
break;
case IRI_JOINTS_MODULE_POINT_WAIT:
@@ -192,6 +222,65 @@ void CIriJointsModule::state_machine(void)
this->track_action.cancel();
}
break;
+ case IRI_JOINTS_MODULE_MOVE_ANGLES:
+ switch (this->move_to_angle_action.get_state())
+ {
+ case ACTION_IDLE:
+ case ACTION_RUNNING:
+ ROS_DEBUG("CIriJointsModule : MoveToAngles running");
+ this->status = IRI_JOINTS_MODULE_RUNNING;
+ break;
+ case ACTION_SUCCESS:
+ ROS_INFO("CIriJointsModule : MoveToAngles ended successfully");
+ this->state = IRI_JOINTS_MODULE_IDLE;
+ this->status = IRI_JOINTS_MODULE_SUCCESS;
+ this->move_to_angle_action.stop_timeout();
+ break;
+ case ACTION_TIMEOUT:
+ ROS_ERROR("CIriJointsModule : MoveToAngles did not finish in the allowed time");
+ this->move_to_angle_action.cancel();
+ this->state = IRI_JOINTS_MODULE_IDLE;
+ this->status = IRI_JOINTS_MODULE_TIMEOUT;
+ this->move_to_angle_action.stop_timeout();
+ break;
+ case ACTION_FB_WATCHDOG:
+ ROS_ERROR("CIriJointsModule : No MoveToAngles's feedback received for a long time");
+ this->move_to_angle_action.cancel();
+ this->state = IRI_JOINTS_MODULE_IDLE;
+ this->status = IRI_JOINTS_MODULE_FB_WATCHDOG;
+ this->move_to_angle_action.stop_timeout();
+ break;
+ case ACTION_ABORTED:
+ ROS_ERROR("CIriJointsModule : MoveToAngles failed to complete");
+ this->state = IRI_JOINTS_MODULE_IDLE;
+ this->status = IRI_JOINTS_MODULE_ABORTED;
+ this->move_to_angle_action.stop_timeout();
+ break;
+ case ACTION_PREEMPTED:
+ ROS_WARN("CIriJointsModule : MoveToAngles was interrupted by another request");
+ this->state = IRI_JOINTS_MODULE_IDLE;//////necesario?????????
+ this->status = IRI_JOINTS_MODULE_PREEMPTED;//////necesario?????????
+ this->move_to_angle_action.stop_timeout();////necesario?????
+ break;
+ case ACTION_REJECTED:
+ ROS_ERROR("CIriJointsModule : MoveToAngles was rejected by server");
+ this->state = IRI_JOINTS_MODULE_IDLE;
+ this->status = IRI_JOINTS_MODULE_REJECTED;
+ this->move_to_angle_action.stop_timeout();
+ break;
+ }
+ if (this->new_angles)
+ {
+ this->state = IRI_JOINTS_MODULE_IDLE;
+ this->status = IRI_JOINTS_MODULE_PREEMPTED;
+ this->move_to_angle_action.stop_timeout();
+ }
+ else if (this->cancel_move_to_angles_pending)
+ {
+ this->cancel_move_to_angles_pending = false;
+ this->move_to_angle_action.cancel();
+ }
+ break;
}
}
@@ -258,6 +347,17 @@ void CIriJointsModule::reconfigure_callback(iri_joints_module::IriJointsModuleCo
else
this->track_action.disable_timeout();
+ /*Move to angles Action parameters*/
+ this->move_to_angle_action.set_max_num_retries(config.move_to_angles_max_retries);
+ if(config.move_to_angles_enable_watchdog)
+ this->move_to_angle_action.enable_watchdog(config.move_to_angles_feedback_watchdog_time_s);
+ else
+ this->move_to_angle_action.disable_watchdog();
+ if(this->config.move_to_angles_enable_timeout)
+ this->move_to_angle_action.enable_timeout(config.move_to_angles_timeout_s);
+ else
+ this->move_to_angle_action.disable_timeout();
+
this->unlock();
}
@@ -288,6 +388,12 @@ bool CIriJointsModule::point_to(double x, double y, double z, double max_vel)
this->unlock();
return false;
}
+ else if (this->state == IRI_JOINTS_MODULE_MOVE_ANGLES)
+ {
+ ROS_WARN("CIriJointsModule: MoveToAngles is active. Dropping this goal.");
+ this->unlock();
+ return false;
+ }
this->point_to_goal.target.point.x = x;
this->point_to_goal.target.point.y = y;
this->point_to_goal.target.point.z = z;
@@ -308,10 +414,10 @@ void CIriJointsModule::cancel_point_to(void)
bool CIriJointsModule::is_point_to_finished(void)
{
- if(this->state == IRI_JOINTS_MODULE_TRACKING || (this->state == IRI_JOINTS_MODULE_IDLE && this->new_point_goal == false))
- return true;
- else
+ if(this->state == IRI_JOINTS_MODULE_POINT_WAIT || (this->state == IRI_JOINTS_MODULE_IDLE && this->new_point_goal))
return false;
+ else
+ return true;
}
/*Tracking action*/
@@ -324,6 +430,12 @@ bool CIriJointsModule::update_target(double x, double y, double z)
this->unlock();
return false;
}
+ else if (this->state == IRI_JOINTS_MODULE_MOVE_ANGLES)
+ {
+ ROS_WARN("CIriJointsModule: MoveToAngles is active. Dropping this goal.");
+ this->unlock();
+ return false;
+ }
else if (this->state == IRI_JOINTS_MODULE_IDLE)
{
ROS_WARN("CIriJointsModule: Tracking is not active. Dropping this goal.");
@@ -343,6 +455,8 @@ void CIriJointsModule::enable_tracking(void)
{
if (this->state == IRI_JOINTS_MODULE_POINT_WAIT)
ROS_ERROR("CIriJointsModule: PointTo action is active. Cancel PointTo before start tracking.");
+ else if (this->state == IRI_JOINTS_MODULE_MOVE_ANGLES)
+ ROS_ERROR("CIriJointsModule: MoveToAngles action is active. Cancel MoveToAngles before start tracking.");
else
this->enable_tracker_pending = true;
}
@@ -357,10 +471,71 @@ void CIriJointsModule::cancel_tracking(void)
bool CIriJointsModule::is_tracking_finished(void)
{
- if(this->state == IRI_JOINTS_MODULE_POINT_WAIT || (this->state == IRI_JOINTS_MODULE_IDLE && this->enable_tracker_pending == false))
+ if(this->state == IRI_JOINTS_MODULE_TRACKING || (this->state == IRI_JOINTS_MODULE_IDLE && this->enable_tracker_pending))
+ return false;
+ else
return true;
+}
+
+/*Move to angles action*/
+bool CIriJointsModule::move_to_angles(std::vector<std::string> joint_names, std::vector<double> angles, std::vector<double> vel)
+{
+ this->lock();
+ if (this->state == IRI_JOINTS_MODULE_POINT_WAIT)
+ {
+ ROS_WARN("CIriJointsModule: PointTo action is active. Dropping this goal.");
+ this->unlock();
+ return false;
+ }
+ else if (this->state == IRI_JOINTS_MODULE_TRACKING)
+ {
+ ROS_WARN("CIriJointsModule: Tracking is active. Dropping this goal.");
+ this->unlock();
+ return false;
+ }
+ if (joint_names.size() != angles.size() || joint_names.size() != vel.size())
+ {
+ ROS_ERROR("CIriJointsModule: The size of joint_names, angles and vel must be the same.");
+ this->unlock();
+ return false;
+ }
+
+ this->move_to_angle_goal.trajectory.header.frame_id = this->config.frame_id;
+ std::vector<std::string>().swap(this->move_to_angle_goal.trajectory.joint_names);
+ this->move_to_angle_goal.trajectory.joint_names = joint_names;
+
+ trajectory_msgs::JointTrajectoryPoint point;
+ point.time_from_start = ros::Duration(this->config.move_to_angles_time_from_start);
+ // ROS_INFO_STREAM("Max time " << point.time_from_start);
+ for (uint8_t i = 0; i < angles.size(); i++)
+ {
+ point.positions.push_back(angles[i]);
+ point.velocities.push_back(vel[i]);
+ //Just for tracking
+ // ROS_INFO_STREAM(" joint[" << (int)i << "] error -> " << error[i] << "; vel -> " << error[i]/duration << "; time -> " << );
+ }
+ std::vector<trajectory_msgs::JointTrajectoryPoint>().swap(this->move_to_angle_goal.trajectory.points);
+ this->move_to_angle_goal.trajectory.points.push_back(point);
+ this->move_to_angle_goal.trajectory.header.stamp = ros::Time::now();
+ this->new_angles = true;
+ this->unlock();
+ return true;
+}
+
+void CIriJointsModule::cancel_move_to_angles(void)
+{
+ if(this->state == IRI_JOINTS_MODULE_MOVE_ANGLES)
+ this->cancel_move_to_angles_pending = true;
else
+ ROS_WARN("CIriJointsModule: MoveToAngles can't be cancelled because is not active.");
+}
+
+bool CIriJointsModule::is_move_to_angles_finished(void)
+{
+ if(this->state == IRI_JOINTS_MODULE_MOVE_ANGLES || (this->state == IRI_JOINTS_MODULE_IDLE && this->new_angles))
return false;
+ else
+ return true;
}
/*Common*/