Error when sending from TrajClient

edit

Hello all,

I am trying to control my robot arm and send it commands realized through a simple trajectory client and a trajectory execution server. I have my client connected to the server but as soon as I send a command I get this error:

right_joint_trajectory_action: /usr/include/boost/smart_ptr/shared_ptr.hpp:418: T* boost::shared_ptr<T>::operator->() const [with T = const control_msgs::FollowJointTrajectoryFeedback_<std::allocator<void> >]: Assertion `px != 0' failed.
[right_joint_trajectory_action-16] process has died [pid 15955, exit code -6, cmd /root/ros_workspace/DrRobotH20/drrobot_jaguar4x4_player/bin/right_joint_trajectory_action __name:=right_joint_trajectory_action __log:=/root/.ros/log/b718d428-70ac-11e2-859d-d067e5ea9c5a/right_joint_trajectory_action-16.log].
log file: /root/.ros/log/b718d428-70ac-11e2-859d-d067e5ea9c5a/right_joint_trajectory_action-16*.log

Interestingly enough, when I use the warehouse planner with my real robot data, as soon as I try to "execute on robot" I get the same error! I believe there is a small bug in my code but I can't find it.

Below I have posted the joint_trajectory_action server code (for my robot's right arm) and the action client code:

ACTION SERVER:

#include <ros/ros.h>
#include <actionlib/server/action_server.h>

#include <trajectory_msgs/JointTrajectory.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <control_msgs/FollowJointTrajectoryFeedback.h>

const double DEFAULT_GOAL_THRESHOLD = 0.01;

class JointTrajectoryExecuter
{
private:
  typedef actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> JTAS;
  typedef JTAS::GoalHandle GoalHandle;
public:
  JointTrajectoryExecuter(ros::NodeHandle &n) :
    node_(n),
    action_server_(node_, "r_arm_controller/joint_trajectory_action",
                   boost::bind(&JointTrajectoryExecuter::goalCB, this, _1),
                   boost::bind(&JointTrajectoryExecuter::cancelCB, this, _1),
                   false),
    has_active_goal_(false)
  {
    using namespace XmlRpc;
    ros::NodeHandle pn("~");

    joint_names_.push_back("base_to_right_shoulder");
    joint_names_.push_back("right_rotator_to_tricep");
    joint_names_.push_back("right_upper_to_lower_bicep");
    joint_names_.push_back("right_lower_bicep_to_upper_forearm");
    joint_names_.push_back("right_lower_forearm_to_wrist");
    joint_names_.push_back("right_wrist_to_hand");




    pn.param("constraints/goal_time", goal_time_constraint_, 0.0);

    // Gets the constraints for each joint.
    for (size_t i = 0; i < joint_names_.size(); ++i)
    {
      std::string ns = std::string("constraints/") + joint_names_[i];
      double g, t;
      pn.param(ns + "/goal", g, DEFAULT_GOAL_THRESHOLD);
      pn.param(ns + "/trajectory", t, -1.0);
      goal_constraints_[joint_names_[i]] = g;
      trajectory_constraints_[joint_names_[i]] = t;
    }
    pn.param("constraints/stopped_velocity_tolerance", stopped_velocity_tolerance_, 0.01);


    pub_controller_command_ =
      node_.advertise<trajectory_msgs::JointTrajectory>("command", 1);
    sub_controller_state_ =
      node_.subscribe("feedback_states", 1, &JointTrajectoryExecuter::controllerStateCB, this);

    action_server_.start();
  }

  ~JointTrajectoryExecuter()
  {
   pub_controller_command_.shutdown();
    sub_controller_state_.shutdown();
    watchdog_timer_.stop();
  }

  bool withinGoalConstraints(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &msg,
                             const std::map<std::string, double>& constraints,
                             const trajectory_msgs::JointTrajectory& traj) {
    ROS_DEBUG("Checking goal contraints");
    int last = traj.points.size() - 1;
    for (size_t i = 0; i < msg->joint_names.size(); ++i)
    {
      double abs_error = fabs(msg->actual.positions[i]-traj.points[last].positions[i]);
      double goal_constraint = constraints.at(msg->joint_names[i]);
      if (goal_constraint >= 0 && abs_error > goal_constraint)
      {
    ROS_DEBUG("Bad constraint: %f, abs_errs: %f", goal_constraint, abs_error);
        return false;
      }
      ROS_DEBUG("Checking constraint: %f, abs_errs: %f", goal_constraint, abs_error);
    }
    return true;
  }

private:

  static bool setsEqual(const std::vector<std::string> &a, const std::vector<std::string> &b)
  {
    if (a.size() != b.size())
      return false;

    for (size_t i = 0; i < a.size(); ++i)
    {
      if (count(b.begin(), b.end(), a[i]) != 1)
        return false;
    }
    for (size_t i = 0; i < b.size(); ++i)
    {
      if (count(a.begin(), a.end(), b[i]) != 1)
        return false;
    }

    return true;
  }

  void watchdog(const ros::TimerEvent &e)
  {
    ros::Time now = ros::Time::now();

    // Aborts the active goal ...