PR2 head control segmentation fault

edit

Hello !

I'm running PR2 in simulation with visual SLAM algorithm (in ROS electric), and a strange behaviour appears when I spawn the robot as his head falls :

When I did the simple_head tutorial, duplicate the point head node to create a raise head node that uses the lookAt method to set the head in the right direction. Everything compiled and worked well till yesterday. I changed a value in the call to lookAt to ajust the direction and compiled again, but now the node is segfaulting at launch. Investigating a bit more showed that the segfault is in instantiating the PointHeadClient in constructor of RobotHead class. I checked actionlib, controller messages, but they're built so they should be effective.

I think the problem may be correlated as I moved from boost 1.40 to 1.49 by copying files in boost_1_49_0/boost in /usr/include/boost as property tree are required by the visual SLAM I use.

Any advice is welcome :)

Thanks, Erwan

EDIT : here is the code :

#include <ros/ros.h>

#include <actionlib/client/simple_action_client.h>
#include <pr2_controllers_msgs/PointHeadAction.h>



// Our Action interface type, provided as a typedef for convenience
typedef actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction> PointHeadClient;

class RobotHead
{
 private:
 PointHeadClient* point_head_client_;

public:
//! Action client initialization 
 RobotHead()
{
  //Initialize the client for the Action interface to the head controller
  point_head_client_ = new PointHeadClient("/head_traj_controller/point_head_action", true);
 std::cout << point_head_client_ << std::endl;
 //wait for head controller action server to come up 
 while(!point_head_client_->waitForServer(ros::Duration(5.0)))
 {
  ROS_INFO("Waiting for the point_head_action server to come up");
  }
 }

~RobotHead()
{
   delete point_head_client_;
}

 //! Points the high-def camera frame at a point in a given frame  
 void lookAt(std::string frame_id, double x, double y, double z)
{
   //the goal message we will be sending
    pr2_controllers_msgs::PointHeadGoal goal;

    //the target point, expressed in the requested frame
     geometry_msgs::PointStamped point;
    point.header.frame_id = frame_id;
    point.point.x = x; point.point.y = y; point.point.z = z;
    goal.target = point;

    //we are pointing the high-def camera frame 
    //(pointing_axis defaults to X-axis)
    goal.pointing_frame = "high_def_frame";

    //take at least 0.5 seconds to get there
    goal.min_duration = ros::Duration(0.5);

    //and go no faster than 1 rad/s
    goal.max_velocity = 1.0;

    //send the goal
   point_head_client_->sendGoal(goal);

   //wait for it to get there (abort after 2 secs to prevent getting stuck)
    point_head_client_->waitForResult(ros::Duration(2));
 }

  //! Shake the head from left to right n times  
  void shakeHead(int n)
 {
   int count = 0;
   while (ros::ok() && ++count <= n )
    {
      //Looks at a point forward (x=5m), slightly left (y=1m), and 1.2m up
       lookAt("base_link", 5.0, 1.0, 1.2);

      //Looks at a point forward (x=5m), slightly right (y=-1m), and 1.2m up
      lookAt("base_link", 5.0, -1.0, 1.2);
    }
  }
};

int main(int argc, char** argv)
{
   //init the ROS node
    ros::init(argc, argv, "robot_driver");

  RobotHead head;
  head.shakeHead(3);
  // The only difference in raisehead is that the previous line is replaced by :
  // head.lookAt("base_link", 2.0, 0.0, 0.8);
}