From 49f16098e7d0b4aedd3f680536762b219f1f2995 Mon Sep 17 00:00:00 2001 From: "SIDDARTH.D" Date: Mon, 31 Jan 2022 09:20:24 +0530 Subject: [PATCH 1/5] Update virtual_box.cpp --- rusty/src/virtual_box.cpp | 161 +++++++++++++++++++++++--------------- 1 file changed, 100 insertions(+), 61 deletions(-) diff --git a/rusty/src/virtual_box.cpp b/rusty/src/virtual_box.cpp index 504e085..1747f0e 100644 --- a/rusty/src/virtual_box.cpp +++ b/rusty/src/virtual_box.cpp @@ -1,82 +1,121 @@ #include #include +// #include +#include -#include -#include +u_int8_t state=0; - - -float po; -void posecallback(const nav_msgs::Odometry::ConstPtr& msg) +void goalCallback(const std_msgs::UInt8::ConstPtr& msg) { - po=msg->pose.pose.position.x; - std::cout<<"POSITION:"<data; + return; } -int main(int argc,char **argv) +int main( int argc, char** argv ) { - ros::init(argc, argv, "basic_shapes"); + ros::init(argc, argv, "add_markers"); + ros::NodeHandle n; - ros::Rate r(1); + ros::Rate r(5); ros::Publisher marker_pub = n.advertise("visualization_marker", 1); + ros::Subscriber sub = n.subscribe("/goal", 1, goalCallback); + bool service_done = false; - ros::Subscriber sub=n.subscribe("odom",10,posecallback); - + // Set our initial shape type to be a cube uint32_t shape = visualization_msgs::Marker::CUBE; - float position_arr[2]={-6.0,10.0}; - for (int i=0;i<3;i++) - { - if(po<=position_arr[i]) + + ROS_INFO("Subscribed to desired goal-position"); + + while (ros::ok()) { + + ros::spinOnce(); + visualization_msgs::Marker marker; + + marker.header.frame_id = "/map"; + marker.header.stamp = ros::Time::now(); + + + marker.ns = "Delivery_box"; + marker.id = 0; + + marker.type = shape; + + marker.scale.x = 0.5; + marker.scale.y = 0.5; + marker.scale.z = 0.5; + + + + marker.color.r = 0.0f; + marker.color.g = 0.0f; + marker.color.b = 1.0f; + marker.color.a = 1.0; + + switch (state) { - while (ros::ok()) + case 0: { - visualization_msgs::Marker marker; - marker.header.frame_id = "/map"; - marker.header.stamp = ros::Time::now(); - marker.ns = "BOX"; - marker.id = 0; - marker.type = shape; marker.action = visualization_msgs::Marker::ADD; + + n.getParam("/pick_up/x", marker.pose.position.x); + + n.getParam("/pick_up/w", marker.pose.orientation.w); + break; + } - marker.pose.position.x = po; - ROS_INFO("POSITION:%f",po); - marker.pose.position.y = 0; - marker.pose.position.z = 0; - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - - marker.scale.x = 3.0; - marker.scale.y = 3.0; - marker.scale.z = 2.0; - - marker.color.r = 0.0f; - marker.color.g = 1.0f; - marker.color.b = 0.0f; - marker.color.a = 1.0; - marker.lifetime = ros::Duration(20); - - while (marker_pub.getNumSubscribers() >= 1) + case 1: { - if (!ros::ok()) - { - return 0; - } - ROS_WARN_ONCE("Please create a subscriber to the marker"); - sleep(1); + sleep(2); + + marker.action = visualization_msgs::Marker::DELETE; + break; + } + + case 2: + { + marker.action = visualization_msgs::Marker::DELETE; + break; + } + + case 3: + { + sleep(5); + + marker.action = visualization_msgs::Marker::ADD; + + n.getParam("/drop/x", marker.pose.position.x); + + n.getParam("/drop/w", marker.pose.orientation.w); + service_done = true; + break; } - marker_pub.publish(marker); - r.sleep(); - } - - ros::Duration(5.0).sleep(); - + } + + + while (marker_pub.getNumSubscribers() < 1) + { + if (!ros::ok()) + { + return 0; + } + ROS_WARN("Please create a subscriber to the marker"); + sleep(1); } - } - - - -} + + + marker_pub.publish(marker); + + + if (service_done) { + ROS_INFO(" Reached"); + sleep(7); + return 0; + } + + r.sleep(); + } + + return 0; +} From a13bafb4195ee2e70df74d14c924ce8602ae9000 Mon Sep 17 00:00:00 2001 From: "SIDDARTH.D" Date: Mon, 31 Jan 2022 09:24:15 +0530 Subject: [PATCH 2/5] Add files via upload --- rusty/config/location.yaml | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 rusty/config/location.yaml diff --git a/rusty/config/location.yaml b/rusty/config/location.yaml new file mode 100644 index 0000000..a186259 --- /dev/null +++ b/rusty/config/location.yaml @@ -0,0 +1,7 @@ +pick_up: + x: 10.0 + w: 1.0 + +drop: + x: -9.0 + w: 1.0 \ No newline at end of file From bb29eb41a415f53e66954b0afc9e8481221e31e3 Mon Sep 17 00:00:00 2001 From: "SIDDARTH.D" Date: Mon, 31 Jan 2022 09:24:47 +0530 Subject: [PATCH 3/5] Update navigation.launch --- rusty/launch/navigation.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rusty/launch/navigation.launch b/rusty/launch/navigation.launch index 7458e47..22cdf11 100644 --- a/rusty/launch/navigation.launch +++ b/rusty/launch/navigation.launch @@ -19,7 +19,7 @@ - + From eb608c08e2bf328c27b691ecea2864cbe562f4b4 Mon Sep 17 00:00:00 2001 From: "SIDDARTH.D" Date: Mon, 31 Jan 2022 09:25:32 +0530 Subject: [PATCH 4/5] Update pick_obj.cpp --- rusty/src/pick_obj.cpp | 81 +++++++++++++++++++++++------------------- 1 file changed, 44 insertions(+), 37 deletions(-) diff --git a/rusty/src/pick_obj.cpp b/rusty/src/pick_obj.cpp index 1811cd8..25e82b6 100644 --- a/rusty/src/pick_obj.cpp +++ b/rusty/src/pick_obj.cpp @@ -1,62 +1,69 @@ #include #include #include -#include +#include -// Define a client for to send goal requests to the move_base server through a SimpleActionClient typedef actionlib::SimpleActionClient MoveBaseClient; -float pose(float pose) -{ - MoveBaseClient ac("move_base", true); - //float pose_arr[2]={-15.0,10.0}; +int main(int argc, char** argv){ + // Initialize the simple_navigation_goals node + ros::init(argc, argv, "pick_objects"); + ros::NodeHandle n; + MoveBaseClient ac("move_base", true); + //set up publisher to broadcast if robot is at goal marker + ros::Publisher pub = n.advertise("/goal", 1); // Wait 5 sec for move_base action server to come up - while(!ac.waitForServer(ros::Duration(5.0))) - { + while(!ac.waitForServer(ros::Duration(5.0))){ ROS_INFO("Waiting for the move_base action server to come up"); } - move_base_msgs::MoveBaseGoal goal; + move_base_msgs::MoveBaseGoal goal; + std_msgs::UInt8 delivery_data; + ROS_INFO("PARAM loaded"); goal.target_pose.header.frame_id = "map"; goal.target_pose.header.stamp = ros::Time::now(); - - ROS_INFO("GOING TO POSITION %f",pose); - goal.target_pose.pose.position.x = pose; - goal.target_pose.pose.orientation.w = 1.0; - - + n.getParam("/pick_up/x", goal.target_pose.pose.position.x); + n.getParam("/pick_up/w", goal.target_pose.pose.orientation.w); - // Send the goal position and orientation for the robot to reach - ROS_INFO("Sending goal to rusty"); ac.sendGoal(goal); - // Wait an infinite time for the results ac.waitForResult(); - // Check if the robot reached its goal - if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) - ROS_INFO("Rusty as reached"); - else - ROS_INFO("Rusty could not reach "); + if (ac.getState()==actionlib::SimpleClientGoalState::SUCCEEDED) + { + delivery_data.data = 1; + pub.publish(delivery_data); + ROS_INFO("PICK UP REACHED"); + } + else{ + delivery_data.data = 2; + pub.publish(delivery_data); + ROS_INFO("Rusty did not reach the location"); + } - return 0; -} + //DROP Sequence + ros::Duration(5).sleep(); + n.getParam("/drop/x", goal.target_pose.pose.position.x); + n.getParam("/drop/w", goal.target_pose.pose.orientation.w); + ac.sendGoal(goal); -int main(int argc, char** argv) -{ - // Initialize the simple_navigation_goals node - ros::init(argc, argv, "pick_drop"); - ROS_INFO("PICK GOAL"); - pose(-6.0); - ros::Duration(0.5).sleep(); - ROS_INFO("DROP GOAL"); - pose(10.0); - - + ac.waitForResult(); + + if (ac.getState()==actionlib::SimpleClientGoalState::SUCCEEDED) + { + delivery_data.data = 3; + pub.publish(delivery_data); + ROS_INFO("PICK UP REACHED"); + } + else{ + delivery_data.data = 2; + pub.publish(delivery_data); + ROS_INFO("Rusty did not reach the location"); + } return 0; -} \ No newline at end of file +} From 321f8544dbe9e78bad150ac251e8a57975fddba0 Mon Sep 17 00:00:00 2001 From: "SIDDARTH.D" Date: Mon, 31 Jan 2022 09:26:50 +0530 Subject: [PATCH 5/5] Update dwa_local_planner_params.yaml --- rusty/params/dwa_local_planner_params.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rusty/params/dwa_local_planner_params.yaml b/rusty/params/dwa_local_planner_params.yaml index fef850f..92017ca 100644 --- a/rusty/params/dwa_local_planner_params.yaml +++ b/rusty/params/dwa_local_planner_params.yaml @@ -1,7 +1,7 @@ DWAPlannerROS: # Robot Configuration Parameters - max_vel_x: 0.35 + max_vel_x: 0.30 min_vel_x: -0.20 max_vel_y: 0.0 @@ -11,8 +11,8 @@ DWAPlannerROS: max_vel_trans: 0.35 min_vel_trans: 0.13 - max_vel_theta: 2.8 - min_vel_theta: 1.5 + max_vel_theta: 1.5 + min_vel_theta: 0.4 acc_lim_x: 2.5 acc_lim_y: 0.0