From c++ code to python code for a ROS program
Hello, I am not good in C++ (Not also good in Python, but better than in C++) I have a code in c++ which I would like to translate in Python but I don't have any idea of where to start (at least I already changed the imported packages) Can someone please help me providing me information about how to start or where to check (I already looked at ROS manuals about creating a publisher and a subscriber but this program has both of them in the same code and I didn't understand it)? It would be very good for me in order to start using it better
The following is the code (it's just a robot which moves with linnear speed of 0.2 and no angular speed (also stops before going to kick a wall)
Includes to talk with ROS and all the other nodes
import rospy from geometry.msgs import Twist from sensor.msgs import LaserScan
Class definition
class Move_and_stop {
public:
Move_and_stop();
void laserCallback(const sensor_msgs::LaserScanConstPtr& m_scan_data);
void emergencyStop();
void calculateCommand();
void mainLoop();
protected:
# Nodehandle for Move_and_stop robot
ros::NodeHandle m_nodeHandle;
# Subscriber and membervariables for our laserscanner
ros::Subscriber m_laserSubscriber;
sensor_msgs::LaserScan m_laserscan;
# Publisher and membervariables for the driving commands
ros::Publisher m_commandPublisher;
geometry_msgs::Twist m_roombaCommand;
};
constructor
Move_and_stop::Move_and_stop() { # Node will be instantiated in root-namespace ros::NodeHandle m_nodeHandle("/");
# Initialising the node handle
m_laserSubscriber = m_nodeHandle.subscribe<sensor_msgs::LaserScan>("laserscan", 20, &Move_and_stop::laserCallback, this);
m_commandPublisher = m_nodeHandle.advertise<geometry_msgs::Twist> ("cmd_vel", 20);
}# end of Move_and_stop constructor
callback to get laser values
void Move_and_stop::laserCallback(const sensor_msgs::LaserScanConstPtr& scanData) {
if( (&m_laserscan)->ranges.size() < scanData->ranges.size() ) {
m_laserscan.ranges.resize((size_t)scanData->ranges.size());
}
for(unsigned int i = 0; i < scanData->ranges.size(); i++)
{
m_laserscan.ranges[i] = scanData->ranges[i];
}
}
robot shall stop, in case anything is closer than 0.6 m
void Move_and_stop::emergencyStop() {
if( (&m_laserscan)->ranges.size() > 0)
{
for(unsigned int i=0; i < (&m_laserscan)->ranges.size(); i++)
{
if( m_laserscan.ranges[i] <= 0.60) {
m_roombaCommand.linear.x = 0.0;
m_roombaCommand.angular.z = 0.0;
}
}
}
}
void Move_and_stop::calculateCommand() {
m_roombaCommand.linear.x = 0.2;
m_roombaCommand.angular.z = 0.0;
}
void Move_and_stop::mainLoop() { # determines the number of loops per second ros::Rate loop_rate(20);
# als long as all is O.K : run
# terminate if the node get a kill signal
while (m_nodeHandle.ok())
{
calculateCommand();
emergencyStop();
ROS_INFO(" robo_01 is moving with speed: forward=%f and turn=%f", m_roombaCommand.linear.x, m_roombaCommand.angular.z);
# send the command to the roomrider for execution
m_commandPublisher.publish(m_roombaCommand);
# spinOnce, just make the loop happen once
ros::spinOnce();
# and sleep, to be aligned with the 50ms loop rate
loop_rate.sleep();
}
}# end of mainLoop
int main(int argc, char** argv) {
ros::init(argc, argv, "Move_and_stop");
Move_and_stop robbi;
robbi.mainLoop();
return 0;
}