35 #include <std_msgs/Time.h>
36 #include <geometry_msgs/PointStamped.h>
37 #include <visualization_msgs/Marker.h>
38 #include <tf/transform_listener.h>
40 #include <sys/types.h>
47 #include <pressure_cells/SenVal.h>
48 #include <pressure_cells/Cop.h>
80 for ( uint i = 0; i < 4; i++ )
90 COP1.header.stamp = ros::Time::now ( );
97 visualization_msgs::Marker Cop_l;
101 Cop_l.header.stamp = ros::Time::now();
104 Cop_l.type = visualization_msgs::Marker::SPHERE;
105 Cop_l.action = visualization_msgs::Marker::ADD;
106 Cop_l.scale.x = 0.02;
107 Cop_l.scale.y = 0.02;
108 Cop_l.scale.z = 0.02;
114 Cop_l.pose.position.x =
COP1.copx;
115 Cop_l.pose.position.y =
COP1.copy;
116 Cop_l.pose.position.z = 0;
117 Cop_l.pose.orientation.x = 0.0;
118 Cop_l.pose.orientation.y = 0.0;
119 Cop_l.pose.orientation.z = 0.0;
120 Cop_l.pose.orientation.w = 1.0;
141 for ( uint i = 0; i < 4; i++ )
152 COP2.header.stamp = ros::Time::now ( );
158 visualization_msgs::Marker Cop_r;
162 Cop_r.header.stamp = ros::Time::now();
165 Cop_r.type = visualization_msgs::Marker::SPHERE;
166 Cop_r.action = visualization_msgs::Marker::ADD;
167 Cop_r.scale.x = 0.02;
168 Cop_r.scale.y = 0.02;
169 Cop_r.scale.z = 0.02;
175 Cop_r.pose.position.x =
COP2.copx;
176 Cop_r.pose.position.y =
COP2.copy;
177 Cop_r.pose.position.z = 0;
178 Cop_r.pose.orientation.x = 0.0;
179 Cop_r.pose.orientation.y = 0.0;
180 Cop_r.pose.orientation.z = 0.0;
181 Cop_r.pose.orientation.w = 1.0;
190 int main (
int argc,
char **argv )
217 ros::init ( argc, argv,
"force_cop" );
219 ros::NodeHandle n(
"~");
221 tf::TransformListener lt;
222 tf::StampedTransform trans_feet;
226 string tf_feet_l_default =
"tf_default_feet_l";
227 n.param(
"tf_feet_l",
tf_feet_l, tf_feet_l_default );
229 string tf_feet_r_default =
"tf_default_feet_r";
230 n.param(
"tf_feet_r",
tf_feet_r, tf_feet_r_default );
234 string markers_default =
"default_markers";
235 n.param(
"markers", markers, markers_default );
237 std::string check_markers;
238 n.getParam(
"markers", check_markers);
240 if (check_markers!=markers_default)
244 ROS_INFO(
"Setting markers parameter to %s", check_markers.c_str());
249 string feet_level_default =
"feet_level_default";
250 n.param(
"feet_level", feet_level, feet_level_default );
252 std::string check_level;
253 n.getParam(
"feet_level", check_level);
255 if (check_level!=feet_level_default)
258 ROS_INFO(
"Setting feet level parameter to %s", check_level.c_str());
261 pub_cop1 = n.advertise < pressure_cells::Cop > (
"/cop1", 1000 );
262 pub_cop2 = n.advertise < pressure_cells::Cop > (
"/cop2", 1000 );
264 ros::Subscriber sub_msg1 = n.subscribe (
"/msg1", 1000,
SenVal1CallBk );
265 ros::Subscriber sub_msg2 = n.subscribe (
"/msg2", 1000,
SenVal2CallBk );
267 ros::Rate loop_rate ( 2.5*1300 );
269 if(
g_lev)
pub_cop = n.advertise < pressure_cells::Cop > (
"/cop", 1000 );
270 if(
g_viz)
pub_viz = n.advertise<visualization_msgs::Marker>( markers, 0 );
284 int pid = getpid(), rpid;
286 boost::format fmt(
"sudo renice -10 %d");
289 rpid = std::system(fmt.str().c_str());
291 while ( ros::ok ( ) )
304 catch (tf::TransformException ex)
306 ROS_INFO(
"Transformation not found: entered catch for 1.5 seconds!");
314 catch (tf::TransformException ex)
316 ROS_ERROR(
"Joystick Transforms: tansforrmation not found after waiting 1.5 seconds\n.%s",ex.what());
321 ROS_ERROR(
"Joystick Transforms: Could not find valid transform after waiting 1.5 seconds\n.%s",ex.what());
326 vector<geometry_msgs::PointStamped> ptrans(4);
328 for (uint n = 0; n < ptrans.size(); n++)
345 for ( i = 0; i < 4; i++ )
347 COP.copx += sen[i] * ptrans[i].point.x;
348 COP.copy += sen[i] * ptrans[i].point.y;
356 COP.copx =
COP.copx / (sen[0] + sen[1] + sen[2] + sen[3] + sen[4] + sen[5] + sen[6] + sen[7]);
358 COP.copy =
COP.copy / (sen[0] + sen[1] + sen[2] + sen[3] + sen[4] + sen[5] + sen[6] + sen[7]);
360 COP.header.stamp = ros::Time::now ( );
365 visualization_msgs::Marker Cop_;
369 Cop_.header.stamp = ros::Time::now();
372 Cop_.type = visualization_msgs::Marker::SPHERE;
373 Cop_.action = visualization_msgs::Marker::ADD;
382 Cop_.pose.position.x =
COP.copx;
383 Cop_.pose.position.y =
COP.copy;
384 Cop_.pose.position.z = 0;
385 Cop_.pose.orientation.x = 0.0;
386 Cop_.pose.orientation.y = 0.0;
387 Cop_.pose.orientation.z = 0.0;
388 Cop_.pose.orientation.w = 1.0;
void SenVal2CallBk(const pressure_cells::SenVal &senval)
vector< geometry_msgs::PointStamped > p_origins_r(4)
Header for communicating w/ the arduino.
vector< geometry_msgs::PointStamped > p_origins_l(4)
int main(int argc, char **argv)
void SenVal1CallBk(const pressure_cells::SenVal &senval)