L1.5/rosaria
6
0
Fork 0
Code Issues 1 Pull Requests Projects Releases Wiki Activity

Removed maxspeed_cb, maxspped_sub and added restrictions_cb, restrictions_sub to handle new type of messages

Browse Source
This commit is contained in:
Pioneer3 2018-09-28 15:10:17 +02:00
parent d6062d98e8
commit 4655f91870
1 changed files with 16 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
RosAria.cpp
View File

@ -29,6 +29,10 @@
#include "std_srvs/Empty.h" #include "std_srvs/Empty.h"
#include "sensor_msgs/JointState.h" #include "sensor_msgs/JointState.h"
#include "rosaria_msgs/RestrictionsMsg.h"
#include "rosaria_msgs/RobotInfoMsg.h"
#include <sstream> #include <sstream>
@ -45,9 +49,7 @@ public:
public: public:
int Setup(); int Setup();
void cmdvel_cb( const geometry_msgs::TwistConstPtr &); void cmdvel_cb( const geometry_msgs::TwistConstPtr &);
void maxspeed_cb( const geometry_msgs::Vector3ConstPtr &); void restrictions_cb( const rosaria_msgs::RestrictionsMsg &);
//void cmd_enable_motors_cb(); //void cmd_enable_motors_cb();
//void cmd_disable_motors_cb(); //void cmd_disable_motors_cb();
@ -77,7 +79,7 @@ protected:
bool published_motors_state; bool published_motors_state;
ros::Subscriber cmdvel_sub; ros::Subscriber cmdvel_sub;
ros::Subscriber maxspeed_sub; ros::Subscriber restrictions_sub;
ros::ServiceServer enable_srv; ros::ServiceServer enable_srv;
ros::ServiceServer disable_srv; ros::ServiceServer disable_srv;
@ -108,6 +110,7 @@ protected:
float maxVel; float maxVel;
float maxRot; float maxRot;
float minDist;
ArRobotConnector *conn; ArRobotConnector *conn;
ArRobot *robot; ArRobot *robot;
@ -367,9 +370,7 @@ RosAriaNode::RosAriaNode(ros::NodeHandle nh) :
cmdvel_sub = n.subscribe( "cmd_vel", 1, (boost::function <void(const geometry_msgs::TwistConstPtr&)>) cmdvel_sub = n.subscribe( "cmd_vel", 1, (boost::function <void(const geometry_msgs::TwistConstPtr&)>)
boost::bind(&RosAriaNode::cmdvel_cb, this, _1 )); boost::bind(&RosAriaNode::cmdvel_cb, this, _1 ));
restrictions_sub = n.subscribe( "/PIONIER/restrictions", 1, &RosAriaNode::restrictions_cb,this);
maxspeed_sub = n.subscribe( "/PIONIER/distance", 1, (boost::function <void(const geometry_msgs::Vector3ConstPtr&)>)
boost::bind(&RosAriaNode::maxspeed_cb, this, _1 ));
// advertise enable/disable services // advertise enable/disable services
enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this); enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this);
@ -389,6 +390,7 @@ RosAriaNode::RosAriaNode(ros::NodeHandle nh) :
maxVel=1.4; maxVel=1.4;
maxRot=1; maxRot=1;
minDist = 0.7;
veltime = ros::Time::now(); veltime = ros::Time::now();
@ -890,14 +892,17 @@ void RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
(double) x * 1e3, (double) y * 1.3, (double) z * 180/M_PI); (double) x * 1e3, (double) y * 1.3, (double) z * 180/M_PI);
} }
void RosAriaNode::maxspeed_cb( const geometry_msgs::Vector3ConstPtr &msg)
void RosAriaNode::restrictions_cb( const rosaria_msgs::RestrictionsMsg &msg)
{ {
veltime = ros::Time::now(); veltime = ros::Time::now();
maxVel=msg->y; maxVel=msg.linear_velocity.data;
maxRot=msg->z; maxRot=msg.angular_velocity.data;
ROS_INFO( "new max speed: [%0.2f,%0.2f]", msg->y, msg->z); minDist=msg.distance.data;
ROS_INFO( "new max speed: [%0.2f,%0.2f]", maxVel, maxRot);
} }
int main( int argc, char** argv ) int main( int argc, char** argv )
{ {
ros::init(argc,argv, "RosAria"); ros::init(argc,argv, "RosAria");