27 #include <visualization_msgs/Marker.h> 28 #include <gazebo_msgs/ContactsState.h> 33 #include <boost/thread/mutex.hpp> 35 #include <std_msgs/Float64.h> 37 #define COUNTDOWN_MAX 5 47 std_msgs::Float64::_data_type
data[5];
58 uint32_t
shape = visualization_msgs::Marker::ARROW;
61 void publish_marker_special(
unsigned int id, std::string framesuffix, const ::geometry_msgs::Vector3 &force,
int col)
63 visualization_msgs::Marker marker;
65 marker.header.frame_id =
"/" + framesuffix;
77 marker.action = visualization_msgs::Marker::ADD;
78 geometry_msgs::Point mypoint;
82 float phalanx_thickness = 0.007;
83 float phalanx_length = 0.01;
85 mypoint.y = phalanx_thickness;
86 mypoint.z = phalanx_length;
87 marker.points.push_back(mypoint);
90 mypoint.x = (force.x / 20.0);
91 mypoint.y = (phalanx_thickness + force.y / 20.0);
92 mypoint.z = phalanx_length + force.z / 20.0;
93 marker.points.push_back(mypoint);
96 marker.scale.x = 0.0025;
97 marker.scale.y = 0.004;
101 marker.color.r =
colors[col][0];
102 marker.color.g =
colors[col][1];
103 marker.color.b =
colors[col][2];
104 marker.color.a = 1.0;
116 visualization_msgs::Marker marker;
118 marker.header.frame_id =
"/srh/position/" + framesuffix;
130 marker.action = visualization_msgs::Marker::ADD;
131 geometry_msgs::Point mypoint;
134 float phalanx_thickness = 0.007;
135 float phalanx_length = 0.01;
137 mypoint.y = phalanx_thickness;
138 mypoint.z = phalanx_length;
139 marker.points.push_back(mypoint);
143 mypoint.y = (phalanx_thickness + force / 20.0);
144 mypoint.z = phalanx_length;
145 marker.points.push_back(mypoint);
148 marker.scale.x = 0.0025;
149 marker.scale.y = 0.004;
153 marker.color.r = (force) > 0.0
f ? (force) : 0.0f;
154 marker.color.g = (1.0f - force) > 0.0
f ? (1.0
f - force) : 0.0f;
155 marker.color.b = 0.0f;
156 marker.color.a = 1.0;
174 for (
unsigned int i = 0; i < msg.states[0].wrenches.size(); ++i)
176 const ::geometry_msgs::Vector3 &v = msg.states[0].wrenches[i].force;
192 for (
unsigned int i = 0; i < msg.states[0].wrenches.size(); i++)
194 const ::geometry_msgs::Vector3 &v = msg.states[0].wrenches[i].force;
210 for (
unsigned int i = 0; i < msg.states[0].wrenches.size(); i++)
212 const ::geometry_msgs::Vector3 &v = msg.states[0].wrenches[i].force;
228 for (
unsigned int i = 0; i < msg.states[0].wrenches.size(); i++)
230 const ::geometry_msgs::Vector3 &v = msg.states[0].wrenches[i].force;
246 for (
unsigned int i = 0; i < msg.states[0].wrenches.size(); i++)
248 const ::geometry_msgs::Vector3 &v = msg.states[0].wrenches[i].force;
256 int main(
int argc,
char **argv)
261 marker_pub = n.
advertise<visualization_msgs::Marker>(
"bumper_markers", 1);
269 std_msgs::Float64::_data_type cur_data[5] = {0};
274 for (
int i = 0; i < 5; i++)
276 cur_data[i] =
data[i];
void callback_mf(const gazebo_msgs::ContactsState &msg)
ros::Publisher marker_pub
std_msgs::Float64::_data_type data[5]
void callback_ff(const gazebo_msgs::ContactsState &msg)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void publish(const boost::shared_ptr< M > &message) const
void callback_lf(const gazebo_msgs::ContactsState &msg)
void publish_marker_special(unsigned int id, std::string framesuffix, const ::geometry_msgs::Vector3 &force, int col)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
boost::mutex update_mutex
int main(int argc, char **argv)
ROSCPP_DECL void spinOnce()
void callback_rf(const gazebo_msgs::ContactsState &msg)
void callback_th(const gazebo_msgs::ContactsState &msg)
void publish_marker(unsigned int id, std::string framesuffix, float force)