# Error when sending from TrajClient

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_ =
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 ...`
edit retag close merge delete

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.

( 2013-02-09 02:12:34 -0600 )edit

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

( 2013-02-09 02:12:49 -0600 )edit

Sort by » oldest newest most voted

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

more

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

more

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)

( 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?!

( 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.

( 2013-10-10 09:10:54 -0600 )edit

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.

more