This class represents a phalange of the hand. It subscribes to gazebo_msgs/ContactsState outputted by the sensor_bumpers and computes the average contact position and normal direction for a given target object. If the phalange is not in contact, the contact pose defaults to a predefined pose. Also, the contact pose expressed in the target object frame is computed. Note, that presently the published geometry_msgs/StampedPose message can create problems when visualizing in Rviz. This is due to the fact that Rviz only buffers 5 geometry_msgs/StampedPose msgs and proper tf transformations might not be available at the time the contact pose is published. This can be fixed by increasing the buffer size in rviz/src/rviz/default_plugin/pose_display.cpp and recompiling Rviz. Currently (in ROS Electric) there is no way to increase Rviz's tf::MessageFilter queue at runtime. Also using a topic_tools/throttle should fix the problem (did not test it though...) More...
#include <phalange.h>
Public Member Functions | |
| std::string | getPhalangeFrameId () |
| std::string | getPhalangeName () |
| icr_msgs::StampedContactPose::ConstPtr | getStampedContactPose () |
| Phalange (tf::Transform const &L_T_Cref, std::string const &phl_name, std::string const &phl_frame_id, std::string const &sensor_topic) | |
| void | setTargetObjFrameId (std::string const &obj_frame_id) |
| ~Phalange () | |
Private Member Functions | |
| void | listenContacts (const icr_msgs::ContactState::ConstPtr &cts_st) |
| Callback listening to ContactsState msgs (which are always published, even if they are empty). Computes the current contact pose. | |
| Phalange () | |
| Using the default constructor is not allowed since the class needs to be initialized properly. This could be changed by implementing corresponding setXYZ methods. | |
| tf::Quaternion | projectPose (tf::Vector3 const &z) |
| Used to generate the contact pose if Phalange::object_model_->geom_ and Phalange::phalange_model_->geom_ are touching. The z-axis of the pose is given as argument. The x & y axis are computed via projecting the x & y axis of the contact reference frame on the nullspace of the new z axis (this is not very stable when axis of the link frame are (nearly) aligned with the z axis of the contact pose since the norms of the projections of those axis will be (nearly) 0) | |
| bool | setPose (icr_msgs::SetPose::Request &req, icr_msgs::SetPose::Response &res) |
| Sets the reference contact pose Phalange::L_T_Cref_ expressed in the phalange link frame. | |
| void | transformContactPose (const boost::shared_ptr< const icr_msgs::StampedContactPose > &C_T_L) |
| Callback registered with the filter; Transforms the current contact pose from the phalange link frame to the object frame. | |
Private Attributes | |
| boost::shared_ptr < icr_msgs::StampedContactPose > | C_T_O_ |
| Current contact pose expressed in the target object frame. Used by the GraspServer class;. | |
| ros::Subscriber | contacts_subs_ |
| Subscribes to a gazebo_msgs/ContactsState message. | |
| ros::Publisher | ct_pose_pub_ |
| Publishes the average contact pose expressed in the phalange link frame if in contact, otherwise a predefined reference pose is published. | |
| tf::Transform | L_T_Cref_ |
| Pose of the reference contact expressed in the link frame id of the phalange. | |
| boost::mutex | lock_ |
| ros::NodeHandle | nh_ |
| ros::NodeHandle | nh_private_ |
| std::string | obj_frame_id_ |
| Defines the target object's frame_id. | |
| std::string | phl_frame_id_ |
| std::string | phl_name_ |
| ros::ServiceServer | set_pose_srv_ |
| Allows to set the pose of the reference contact w.r.t the phalange link frame id (mainly intended for debugging) | |
| tf::MessageFilter < icr_msgs::StampedContactPose > * | tf_filter_ |
| Message filter to ensure a transformation from the phalange link to the target object is available. | |
| tf::TransformListener | tf_list_ |
| std::string | tf_prefix_ |
This class represents a phalange of the hand. It subscribes to gazebo_msgs/ContactsState outputted by the sensor_bumpers and computes the average contact position and normal direction for a given target object. If the phalange is not in contact, the contact pose defaults to a predefined pose. Also, the contact pose expressed in the target object frame is computed. Note, that presently the published geometry_msgs/StampedPose message can create problems when visualizing in Rviz. This is due to the fact that Rviz only buffers 5 geometry_msgs/StampedPose msgs and proper tf transformations might not be available at the time the contact pose is published. This can be fixed by increasing the buffer size in rviz/src/rviz/default_plugin/pose_display.cpp and recompiling Rviz. Currently (in ROS Electric) there is no way to increase Rviz's tf::MessageFilter queue at runtime. Also using a topic_tools/throttle should fix the problem (did not test it though...)
Definition at line 43 of file phalange.h.
| ICR::Phalange::Phalange | ( | tf::Transform const & | L_T_Cref, |
| std::string const & | phl_name, | ||
| std::string const & | phl_frame_id, | ||
| std::string const & | sensor_topic | ||
| ) |
Definition at line 9 of file phalange.cpp.
Definition at line 27 of file phalange.cpp.
| ICR::Phalange::Phalange | ( | ) | [private] |
Using the default constructor is not allowed since the class needs to be initialized properly. This could be changed by implementing corresponding setXYZ methods.
| std::string ICR::Phalange::getPhalangeFrameId | ( | ) |
Definition at line 170 of file phalange.cpp.
| std::string ICR::Phalange::getPhalangeName | ( | ) |
Definition at line 159 of file phalange.cpp.
| icr_msgs::StampedContactPose::ConstPtr ICR::Phalange::getStampedContactPose | ( | ) |
Definition at line 149 of file phalange.cpp.
| void ICR::Phalange::listenContacts | ( | const icr_msgs::ContactState::ConstPtr & | cts_st | ) | [private] |
Callback listening to ContactsState msgs (which are always published, even if they are empty). Computes the current contact pose.
Definition at line 47 of file phalange.cpp.
| tf::Quaternion ICR::Phalange::projectPose | ( | tf::Vector3 const & | z | ) | [private] |
Used to generate the contact pose if Phalange::object_model_->geom_ and Phalange::phalange_model_->geom_ are touching. The z-axis of the pose is given as argument. The x & y axis are computed via projecting the x & y axis of the contact reference frame on the nullspace of the new z axis (this is not very stable when axis of the link frame are (nearly) aligned with the z axis of the contact pose since the norms of the projections of those axis will be (nearly) 0)
Definition at line 104 of file phalange.cpp.
| bool ICR::Phalange::setPose | ( | icr_msgs::SetPose::Request & | req, |
| icr_msgs::SetPose::Response & | res | ||
| ) | [private] |
Sets the reference contact pose Phalange::L_T_Cref_ expressed in the phalange link frame.
Definition at line 135 of file phalange.cpp.
| void ICR::Phalange::setTargetObjFrameId | ( | std::string const & | obj_frame_id | ) |
Definition at line 95 of file phalange.cpp.
| void ICR::Phalange::transformContactPose | ( | const boost::shared_ptr< const icr_msgs::StampedContactPose > & | C_T_L | ) | [private] |
Callback registered with the filter; Transforms the current contact pose from the phalange link frame to the object frame.
Definition at line 29 of file phalange.cpp.
boost::shared_ptr<icr_msgs::StampedContactPose> ICR::Phalange::C_T_O_ [private] |
Current contact pose expressed in the target object frame. Used by the GraspServer class;.
Definition at line 84 of file phalange.h.
ros::Subscriber ICR::Phalange::contacts_subs_ [private] |
Subscribes to a gazebo_msgs/ContactsState message.
Definition at line 72 of file phalange.h.
ros::Publisher ICR::Phalange::ct_pose_pub_ [private] |
Publishes the average contact pose expressed in the phalange link frame if in contact, otherwise a predefined reference pose is published.
Definition at line 68 of file phalange.h.
tf::Transform ICR::Phalange::L_T_Cref_ [private] |
Pose of the reference contact expressed in the link frame id of the phalange.
Definition at line 80 of file phalange.h.
boost::mutex ICR::Phalange::lock_ [private] |
Definition at line 94 of file phalange.h.
ros::NodeHandle ICR::Phalange::nh_ [private] |
Definition at line 64 of file phalange.h.
ros::NodeHandle ICR::Phalange::nh_private_ [private] |
Definition at line 64 of file phalange.h.
std::string ICR::Phalange::obj_frame_id_ [private] |
Defines the target object's frame_id.
Definition at line 92 of file phalange.h.
std::string ICR::Phalange::phl_frame_id_ [private] |
Definition at line 87 of file phalange.h.
std::string ICR::Phalange::phl_name_ [private] |
Definition at line 86 of file phalange.h.
ros::ServiceServer ICR::Phalange::set_pose_srv_ [private] |
Allows to set the pose of the reference contact w.r.t the phalange link frame id (mainly intended for debugging)
Definition at line 76 of file phalange.h.
tf::MessageFilter<icr_msgs::StampedContactPose>* ICR::Phalange::tf_filter_ [private] |
Message filter to ensure a transformation from the phalange link to the target object is available.
Definition at line 100 of file phalange.h.
tf::TransformListener ICR::Phalange::tf_list_ [private] |
Definition at line 95 of file phalange.h.
std::string ICR::Phalange::tf_prefix_ [private] |
Definition at line 102 of file phalange.h.