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 :
falling head http://anusite.free.fr/Documents/stageISIR/pr2_falling_head.png
When I did the [simple_head tutorial](www.ros.org/wiki/pr2_controllers/Tutorials/Moving the Head), 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);
}
Originally posted by Erwan R. on ROS Answers with karma: 697 on 2012-07-02
Post score: 0