Skip to content

Commit

Permalink
package renamed
Browse files Browse the repository at this point in the history
package integration with other packages performed and tested
  • Loading branch information
KushnirDmytro committed Mar 6, 2019
1 parent 0615e30 commit d6e9cc1
Show file tree
Hide file tree
Showing 5 changed files with 30 additions and 30 deletions.
4 changes: 2 additions & 2 deletions explore_stack/launch/combined_bringup.launch
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,8 @@
<arg name="model" value="$(arg model)"/>
</include>

<!--<include file="$(find cmd_vel_watchdog)/launch/cmd_vel_watchdog.launch">-->
<!--</include>-->
<include file="$(find velocity_topic_watchdog)/launch/velocity_topic_watchdog.launch">
</include>


<!--GAZEBO-->
Expand Down
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 2.8.3)
project(cmd_vel_watchdog)
project(velocity_topic_watchdog)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
Expand Down Expand Up @@ -104,7 +104,7 @@ find_package(catkin REQUIRED COMPONENTS
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES cmd_vel_watchdog
# LIBRARIES velocity_topic_watchdog
# CATKIN_DEPENDS geometry_msgs roscpp rospy
# DEPENDS system_lib
)
Expand All @@ -122,7 +122,7 @@ include_directories(

## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/cmd_vel_watchdog.cpp
# src/${PROJECT_NAME}/velocity_topic_watchdog.cpp
# )

## Add cmake target dependencies of the library
Expand All @@ -133,7 +133,7 @@ include_directories(
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(cmd_vel_watchdog src/cmd_vel_watchdog.cpp)
add_executable(velocity_topic_watchdog src/velocity_topic_watchdog.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
Expand All @@ -146,7 +146,7 @@ add_executable(cmd_vel_watchdog src/cmd_vel_watchdog.cpp)
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
target_link_libraries(cmd_vel_watchdog
target_link_libraries(velocity_topic_watchdog
${catkin_LIBRARIES}
)

Expand All @@ -158,7 +158,7 @@ add_executable(cmd_vel_watchdog src/cmd_vel_watchdog.cpp)
## Install ##
#############
# install nodes
install(TARGETS cmd_vel_watchdog
install(TARGETS velocity_topic_watchdog
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
Expand Down
Original file line number Diff line number Diff line change
@@ -1,11 +1,11 @@
<launch>
<arg name="velocity_topic" default="cmd_vel"/>
<node pkg="cmd_vel_watchdog"
type="cmd_vel_watchdog"
<node pkg="velocity_topic_watchdog"
type="velocity_topic_watchdog"
respawn="true"
name="cmd_vel_watchdog"
name="velocity_topic_watchdog"
output="screen">
<remap from="cmd_vel" to="$(arg velocity_topic)"/>
</node>
<param name="watchdog_patience" value="10.0" type="double"/>
<param name="watchdog_patience" value="2.0" type="double"/>
</launch>
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
<?xml version="1.0"?>
<package format="2">
<name>cmd_vel_watchdog</name>
<name>velocity_topic_watchdog</name>
<version>0.0.0</version>
<description>The cmd_vel_watchdog package</description>
<description>The velocity_topic_watchdog package</description>

<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

namespace cmd_vel_watchdog{
namespace velocity_topic_watchdog{

ros::Publisher *pub_ptr;
ros::Timer *global_timer;
Expand All @@ -16,7 +16,7 @@ ros::Duration *watchdog_duration;
double watchdog_timeout_param (3.0);

void watchdogTimerCallback(const ros::TimerEvent &timerEvent){
ROS_WARN_STREAM("velocity topic watchdog activated!");
ROS_WARN_STREAM("velocity_topic_watchdog activated!");
pub_ptr->publish(*stop_msg_ptr);
ROS_WARN_STREAM("STOP MSG PUBLISHED to [" << pub_ptr->getTopic() << "]");
global_timer->stop();
Expand Down Expand Up @@ -63,41 +63,41 @@ void commandVelocityReceived(const geometry_msgs::Twist &msgIn){
}


} // namespace cmd_vel_watchdog
} // namespace velocity_topic_watchdog
int main(int argc, char** argv){
ros::init(argc, argv, "cmd_vel_watchdog");
ros::init(argc, argv, "velocity_topic_watchdog");
ros::NodeHandle nh;

if (!nh.getParam("watchdog_patience", cmd_vel_watchdog::watchdog_timeout_param)){
if (!nh.getParam("watchdog_patience", velocity_topic_watchdog::watchdog_timeout_param)){
ROS_INFO_STREAM(
"watchdog_patience param is not found, setting to default = " <<
cmd_vel_watchdog::DEFAULT_WATCHDOG_TIMEOUT << " sec");
cmd_vel_watchdog::watchdog_timeout_param = cmd_vel_watchdog::DEFAULT_WATCHDOG_TIMEOUT;
velocity_topic_watchdog::DEFAULT_WATCHDOG_TIMEOUT << " sec");
velocity_topic_watchdog::watchdog_timeout_param = velocity_topic_watchdog::DEFAULT_WATCHDOG_TIMEOUT;
}
ROS_INFO_STREAM("watchdog time set to " << cmd_vel_watchdog::watchdog_timeout_param << " [sec]");
ROS_INFO_STREAM("watchdog time set to " << velocity_topic_watchdog::watchdog_timeout_param << " [sec]");

cmd_vel_watchdog::initGlobals(nh);
velocity_topic_watchdog::initGlobals(nh);



const std::string &default_velocity_topic = "cmd_vel";
const std::string &this_node_name = ros::this_node::getName();
ROS_INFO_STREAM(this_node_name << " created");

cmd_vel_watchdog::pub_ptr = new ros::Publisher(nh.advertise<geometry_msgs::Twist>(default_velocity_topic, 1));
velocity_topic_watchdog::pub_ptr = new ros::Publisher(nh.advertise<geometry_msgs::Twist>(default_velocity_topic, 1));

ROS_INFO_STREAM(this_node_name <<" advertised publisher to " << cmd_vel_watchdog::pub_ptr->getTopic());
ROS_INFO_STREAM(this_node_name <<" advertised publisher to " << velocity_topic_watchdog::pub_ptr->getTopic());

ros::Subscriber sub = nh.subscribe(default_velocity_topic, 1, &cmd_vel_watchdog::commandVelocityReceived);
ros::Subscriber sub = nh.subscribe(default_velocity_topic, 1, &velocity_topic_watchdog::commandVelocityReceived);
ROS_INFO_STREAM(this_node_name <<" subscribed to topic " << sub.getTopic());

while(ros::ok()){
ros::spin();
}

ROS_WARN_STREAM(this_node_name <<" SHUT_DOWN ");
delete cmd_vel_watchdog::pub_ptr;
delete cmd_vel_watchdog::global_timer;
delete cmd_vel_watchdog::stop_msg_ptr;
delete cmd_vel_watchdog::watchdog_duration;
delete velocity_topic_watchdog::pub_ptr;
delete velocity_topic_watchdog::global_timer;
delete velocity_topic_watchdog::stop_msg_ptr;
delete velocity_topic_watchdog::watchdog_duration;
}

0 comments on commit d6e9cc1

Please sign in to comment.