void JointMotionHandler::motionInterface(JointMessage & jMsg) { // // Upon receiving... // 1st point - initialize buffer, enable motion, start job, add point (increment buffer index), // Nth point - add point (increment buffer index) // end of trajectory - wait until buffer size = 0, disable motion, stop job, reset buffer indicies //motion stop - disable motion, stop job JointData joints; switch (jMsg.getSequence()) { case SpecialSeqValues::END_TRAJECTORY: LOG_INFO("Received end trajectory command"); while(!pVarQ.bufferEmpty()) { LOG_DEBUG("Waiting for motion buffer to empty"); mpTaskDelay(this->BUFFER_POLL_TICK_DELAY); }; this->stopMotionJob(); break; case SpecialSeqValues::STOP_TRAJECTORY: LOG_WARN("Received stop command, stopping motion immediately"); this->stopMotionJob(); break; default: joints.copyFrom(jMsg.getJoints()); if (!(this->isJobStarted())) { //TODO: The velocity should be set from the message in the future. pVarQ.init(joints, 0.0); this->startMotionJob(); } else { pVarQ.addPoint(joints, 0.0); } } }
bool JointRelayHandler::convert_message(JointMessage& msg_in, JointTrajectoryPoint* joint_state) { // copy position data int num_jnts = all_joint_names_.size(); joint_state->positions.resize(num_jnts); for (int i=0; i<num_jnts; ++i) { shared_real value; if (msg_in.getJoints().getJoint(i, value)) joint_state->positions[i] = value; else LOG_ERROR("Failed to parse position #%d from JointMessage", i); } // these fields are not provided by JointMessage joint_state->velocities.clear(); joint_state->accelerations.clear(); joint_state->time_from_start = ros::Duration(0); return true; }
bool JointRelayHandler::convert_message(JointMessage& msg_in, DynamicJointsGroup* joint_state, int robot_id) { // copy position data int num_jnts = robot_groups_[robot_id].get_joint_names().size(); joint_state->positions.resize(num_jnts); for (int i = 0; i < num_jnts; ++i) { shared_real value; if (msg_in.getJoints().getJoint(i, value)) { joint_state->positions[i] = value; } else LOG_ERROR("Failed to convert message"); } // these fields are not provided by JointMessage joint_state->velocities.clear(); joint_state->accelerations.clear(); joint_state->time_from_start = ros::Duration(0); return true; }
void JointTrajectoryHandler::trajectoryHandler() { JointMessage jMsg; SimpleMessage msg; SimpleMessage reply; trajectory_msgs::JointTrajectoryPoint pt; ROS_INFO("Starting joint trajectory handler state"); while (ros::ok()) { this->mutex_.lock(); if (this->robot_->isConnected()) { //TODO: These variables should be moved outside of this loop //so that we aren't contantly reinitializing them. JointMessage jMsg; SimpleMessage msg; SimpleMessage reply; trajectory_msgs::JointTrajectoryPoint pt; switch (this->state_) { case JointTrajectoryStates::IDLE: ros::Duration(0.250).sleep(); break; case JointTrajectoryStates::STREAMING: if (this->currentPoint < this->current_traj_.points.size()) { // unsigned int lastCurrentPoint = currentPoint; // currentPoint = getNextTrajectoryPoint(current_traj_, // streaming_start_, // ros::Time::now()); //ROS_INFO("Skipping from point[%d] to point[%d]", lastCurrentPoint, currentPoint); pt = this->current_traj_.points[this->currentPoint]; jMsg.setSequence(this->currentPoint); for (int i = 0; i < this->current_traj_.joint_names.size(); i++) { jMsg.getJoints().setJoint(i, pt.positions[i]); } ROS_DEBUG("Sending joint point"); jMsg.toRequest(msg); if (this->robot_->sendAndReceiveMsg(msg, reply, false)) //jMsg.toTopic(msg); //if (this->robot_->sendMsg(msg)) { ROS_INFO("Point[%d of %d] sent to controller", this->currentPoint, this->current_traj_.points.size()); this->currentPoint++; } else { ROS_WARN("Failed sent joint point, will try again"); } //ROS_INFO_STREAM("Time taken to stream single point is " << (ros::WallTime::now()-start)); } else { ROS_INFO("Trajectory streaming complete, setting state to IDLE"); this->state_ = JointTrajectoryStates::IDLE; } break; default: ROS_ERROR("Joint trajectory handler: unknown state"); this->state_ = JointTrajectoryStates::IDLE; break; } } else { ROS_INFO("Connecting to robot motion server"); this->robot_->makeConnect(); } this->mutex_.unlock(); ros::Duration(0.005).sleep(); } ROS_WARN("Exiting trajectory handler thread"); }