Ask Your Question
0

Error when sending from TrajClient

asked 2013-02-06 10:44:56 -0600

MartinW gravatar image

updated 2014-04-20 14:06:46 -0600

ngrennan gravatar image

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 ...
(more)
edit retag flag offensive close merge delete

Comments

Hi Martin, this kind of bug is much easier to find with a debugger such as gdb. Could you upload a .tar archive of your whole package somewhere? Alternatively, run the server in gdb and have a look at the backtrace; this should tell you the line number of the error.

Martin Günther gravatar imageMartin Günther ( 2013-02-09 02:12:34 -0600 )edit

BTW, you shouldn't be running this as root. :-)

Martin Günther gravatar imageMartin Günther ( 2013-02-09 02:12:49 -0600 )edit

3 Answers

Sort by » oldest newest most voted
0

answered 2013-02-09 09:30:15 -0600

MartinW gravatar image

Hello Martin, I figured out the error! It was from not publishing the FollowJointTrajectoryFeedback feeback state :) And why shouldn't I be running on as root? It was how the other guy working in the lab had it, I like not needing to type sudo all the time haha

edit flag offensive delete link more
0

answered 2013-10-09 04:24:43 -0600

AndreD gravatar image

Hello Martin, i am using your sourcecode to run my own robot and it works fine until i push the "execute" button in Moveit. I get the same error as you described. How did you fix the problem in detail? Could you maybe print your code here, so that i can fix my problem? Thank you very much for your efforts.

Kind regards, Andre

edit flag offensive delete link more

Comments

Hello Andre, I believe the answer is in the comments. Are you publishing the FollowJointTrajectoryFeedback information? I think this was the cause of the error (according to my previous comment anyway)

MartinW gravatar imageMartinW ( 2013-10-09 07:49:23 -0600 )edit

Hey, first thanks for your fast answer. Yeah i have read your command, but i dont know exactly where/how to publish the FollowJointTrajectoryFeedback information in the code, because i'm not that experienced in c++ yet;) Is it just one line of code to add or is it more complicated? I guess the action server node has to publish the information?!

AndreD gravatar imageAndreD ( 2013-10-09 22:09:23 -0600 )edit

Yes, I believe you do need an action server node. I made mine similar to the motoman joint_trajectory_action.cpp using fuerte (http://wiki.ros.org/motoman) but you'll need to change all the joint_names and such to your own robots names in the urdf.

MartinW gravatar imageMartinW ( 2013-10-10 09:10:54 -0600 )edit
0

answered 2015-09-14 08:20:45 -0600

Diana gravatar image

Hello I know that this topic was some time ago, but could you explain to me, if I use thhe action server with move_group from moveit. how could I get the FollowJointTrajectoryFeedback if moveit just ignore it.? or the FollowJointTrajectoryFeedback hast to be plublished from the server?? I have check the motoman joint_trajectory_action.cpp in ( http://wiki.ros.org/motoman ), but I don't find the part where FollowJointTrajectoryFeedback is published, this is only subscribed... I hope you can help me, I really need this for my project and I just can not understand very well.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2013-02-06 10:44:56 -0600

Seen: 697 times

Last updated: Sep 14 '15